Skip to content

Fix being unable to change StandardTrajectoryGenerator parameter vtheta_samples #1619

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
SteveMacenski merged 8 commits into from
Aug 21, 2020
Merged

Fix being unable to change StandardTrajectoryGenerator parameter vtheta_samples #1619

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
7704c4f
Fix tests declaring parameters real nodes don't
sloretz Mar 31, 2020
e6a210f
Fix loadParameterWithDeprecation not getting initial parameter values
sloretz Mar 31, 2020
deded44
Create sim_time variable before using it
sloretz Mar 31, 2020
6ef51eb
Line length < 100
sloretz Mar 31, 2020
1211944
Add missing {
sloretz Apr 1, 2020
c3ad9c4
Linter fixes
sloretz Aug 21, 2020
9013a9b
sim_granularity -> time_granularity
sloretz Aug 21, 2020
1ac0b63
Linter fix
sloretz Aug 21, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
107 changes: 55 additions & 52 deletions nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,10 +74,14 @@ std::vector<rclcpp::Parameter> getDefaultKinematicParameters()
return parameters;
}

rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode(const std::string & name)
rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode(
const std::string & name,
const std::vector<rclcpp::Parameter> & overrides = {})
{
rclcpp::NodeOptions node_options = nav2_util::get_node_options_default();
rclcpp::NodeOptions node_options;
node_options.parameter_overrides(getDefaultKinematicParameters());
node_options.parameter_overrides().insert(
node_options.parameter_overrides().end(), overrides.begin(), overrides.end());

auto node = rclcpp_lifecycle::LifecycleNode::make_shared(name, node_options);
node->on_configure(node->get_current_state());
Expand Down Expand Up @@ -142,8 +146,7 @@ TEST(VelocityIterator, standard_gen)

TEST(VelocityIterator, max_xy)
{
auto nh = makeTestNode("max_xy");
nh->set_parameters({rclcpp::Parameter("dwb.max_speed_xy", 1.0)});
auto nh = makeTestNode("max_xy", {rclcpp::Parameter("dwb.max_speed_xy", 1.0)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");

Expand All @@ -155,8 +158,7 @@ TEST(VelocityIterator, max_xy)

TEST(VelocityIterator, min_xy)
{
auto nh = makeTestNode("min_xy");
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
auto nh = makeTestNode("min_xy", {rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
Expand All @@ -167,8 +169,7 @@ TEST(VelocityIterator, min_xy)

TEST(VelocityIterator, min_theta)
{
auto nh = makeTestNode("min_theta");
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
auto nh = makeTestNode("min_theta", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
Expand All @@ -179,10 +180,11 @@ TEST(VelocityIterator, min_theta)

TEST(VelocityIterator, no_limits)
{
auto nh = makeTestNode("no_limits");
nh->set_parameters({rclcpp::Parameter("dwb.max_speed_xy", -1.0)});
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
auto nh = makeTestNode(
"no_limits", {
rclcpp::Parameter("dwb.max_speed_xy", -1.0),
rclcpp::Parameter("dwb.min_speed_xy", -1.0),
rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
Expand All @@ -193,14 +195,15 @@ TEST(VelocityIterator, no_limits)

TEST(VelocityIterator, no_limits_samples)
{
auto nh = makeTestNode("no_limits_samples");
nh->set_parameters({rclcpp::Parameter("dwb.max_speed_xy", -1.0)});
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
int x_samples = 10, y_samples = 3, theta_samples = 5;
nh->set_parameters({rclcpp::Parameter("dwb.vx_samples", x_samples)});
nh->set_parameters({rclcpp::Parameter("dwb.vy_samples", y_samples)});
nh->set_parameters({rclcpp::Parameter("dwb.vtheta_samples", theta_samples)});
const int x_samples = 10, y_samples = 3, theta_samples = 5;
auto nh = makeTestNode(
"no_limits_samples", {
rclcpp::Parameter("dwb.max_speed_xy", -1.0),
rclcpp::Parameter("dwb.min_speed_xy", -1.0),
rclcpp::Parameter("dwb.min_speed_theta", -1.0),
rclcpp::Parameter("dwb.vx_samples", x_samples),
rclcpp::Parameter("dwb.vy_samples", y_samples),
rclcpp::Parameter("dwb.vtheta_samples", theta_samples)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
Expand All @@ -210,8 +213,7 @@ TEST(VelocityIterator, no_limits_samples)

TEST(VelocityIterator, dwa_gen)
{
auto nh = makeTestNode("dwa_gen");
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
auto nh = makeTestNode("dwa_gen", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize(nh, "dwb");
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
Expand All @@ -222,8 +224,7 @@ TEST(VelocityIterator, dwa_gen)

TEST(VelocityIterator, nonzero)
{
auto nh = makeTestNode("nonzero");
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
auto nh = makeTestNode("nonzero", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize(nh, "dwb");
nav_2d_msgs::msg::Twist2D initial;
Expand Down Expand Up @@ -273,9 +274,8 @@ const double DEFAULT_SIM_TIME = 1.7;

TEST(TrajectoryGenerator, basic)
{
auto nh = makeTestNode("basic");
auto nh = makeTestNode("basic", {rclcpp::Parameter("dwb.linear_granularity", 0.5)});
StandardTrajectoryGenerator gen;
nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)});
gen.initialize(nh, "dwb");
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
matchTwist(res.velocity, forward);
Expand All @@ -290,10 +290,11 @@ TEST(TrajectoryGenerator, basic)

TEST(TrajectoryGenerator, basic_no_last_point)
{
auto nh = makeTestNode("basic_no_last_point");
auto nh = makeTestNode(
"basic_no_last_point", {
rclcpp::Parameter("dwb.include_last_point", false),
rclcpp::Parameter("dwb.linear_granularity", 0.5)});
StandardTrajectoryGenerator gen;
nh->set_parameters({rclcpp::Parameter("dwb.include_last_point", false)});
nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)});
gen.initialize(nh, "dwb");
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
matchTwist(res.velocity, forward);
Expand All @@ -308,9 +309,8 @@ TEST(TrajectoryGenerator, basic_no_last_point)

TEST(TrajectoryGenerator, too_slow)
{
auto nh = makeTestNode("too_slow");
auto nh = makeTestNode("too_slow", {rclcpp::Parameter("dwb.linear_granularity", 0.5)});
StandardTrajectoryGenerator gen;
nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)});
gen.initialize(nh, "dwb");
nav_2d_msgs::msg::Twist2D cmd;
cmd.x = 0.2;
Expand All @@ -326,9 +326,8 @@ TEST(TrajectoryGenerator, too_slow)

TEST(TrajectoryGenerator, holonomic)
{
auto nh = makeTestNode("holonomic");
auto nh = makeTestNode("holonomic", {rclcpp::Parameter("dwb.linear_granularity", 0.5)});
StandardTrajectoryGenerator gen;
nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)});
gen.initialize(nh, "dwb");
nav_2d_msgs::msg::Twist2D cmd;
cmd.x = 0.3;
Expand All @@ -346,10 +345,11 @@ TEST(TrajectoryGenerator, holonomic)

TEST(TrajectoryGenerator, twisty)
{
auto nh = makeTestNode("twisty");
auto nh = makeTestNode(
"twisty", {
rclcpp::Parameter("dwb.linear_granularity", 0.5),
rclcpp::Parameter("dwb.angular_granularity", 0.025)});
StandardTrajectoryGenerator gen;
nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)});
nh->set_parameters({rclcpp::Parameter("dwb.angular_granularity", 0.025)});
gen.initialize(nh, "dwb");
nav_2d_msgs::msg::Twist2D cmd;
cmd.x = 0.3;
Expand All @@ -370,10 +370,11 @@ TEST(TrajectoryGenerator, twisty)

TEST(TrajectoryGenerator, sim_time)
{
auto nh = makeTestNode("sim_time");
const double sim_time = 2.5;
nh->set_parameters({rclcpp::Parameter("dwb.sim_time", sim_time)});
nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)});
auto nh = makeTestNode(
"sim_time", {
rclcpp::Parameter("dwb.sim_time", sim_time),
rclcpp::Parameter("dwb.linear_granularity", 0.5)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
Expand All @@ -389,12 +390,13 @@ TEST(TrajectoryGenerator, sim_time)

TEST(TrajectoryGenerator, accel)
{
auto nh = makeTestNode("accel");
nh->set_parameters({rclcpp::Parameter("dwb.sim_time", 5.0)});
nh->set_parameters({rclcpp::Parameter("dwb.discretize_by_time", true)});
nh->set_parameters({rclcpp::Parameter("dwb.time_granularity", 1.0)});
nh->set_parameters({rclcpp::Parameter("dwb.acc_lim_x", 0.1)});
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
auto nh = makeTestNode(
"accel", {
rclcpp::Parameter("dwb.sim_time", 5.0),
rclcpp::Parameter("dwb.discretize_by_time", true),
rclcpp::Parameter("dwb.time_granularity", 1.0),
rclcpp::Parameter("dwb.acc_lim_x", 0.1),
rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
StandardTrajectoryGenerator gen;
gen.initialize(nh, "dwb");

Expand All @@ -412,13 +414,14 @@ TEST(TrajectoryGenerator, accel)

TEST(TrajectoryGenerator, dwa)
{
auto nh = makeTestNode("dwa");
nh->set_parameters({rclcpp::Parameter("dwb.sim_period", 1.0)});
nh->set_parameters({rclcpp::Parameter("dwb.sim_time", 5.0)});
nh->set_parameters({rclcpp::Parameter("dwb.discretize_by_time", true)});
nh->set_parameters({rclcpp::Parameter("dwb.time_granularity", 1.0)});
nh->set_parameters({rclcpp::Parameter("dwb.acc_lim_x", 0.1)});
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
auto nh = makeTestNode(
"dwa", {
rclcpp::Parameter("dwb.sim_period", 1.0),
rclcpp::Parameter("dwb.sim_time", 5.0),
rclcpp::Parameter("dwb.discretize_by_time", true),
rclcpp::Parameter("dwb.time_granularity", 1.0),
rclcpp::Parameter("dwb.acc_lim_x", 0.1),
rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
dwb_plugins::LimitedAccelGenerator gen;
gen.initialize(nh, "dwb");

Expand Down
22 changes: 15 additions & 7 deletions nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,18 +93,26 @@ param_t loadParameterWithDeprecation(
const nav2_util::LifecycleNode::SharedPtr & nh, const std::string current_name,
const std::string old_name, const param_t & default_value)
{
param_t value = 0;
if (nh->get_parameter(current_name, value)) {
return value;
}
if (nh->get_parameter(old_name, value)) {
nav2_util::declare_parameter_if_not_declared(
nh, current_name, rclcpp::ParameterValue(default_value));
nav2_util::declare_parameter_if_not_declared(
nh, old_name, rclcpp::ParameterValue(default_value));

param_t current_name_value;
nh->get_parameter(current_name, current_name_value);
param_t old_name_value;
nh->get_parameter(old_name, old_name_value);

if (old_name_value != current_name_value && old_name_value != default_value) {
RCLCPP_WARN(
nh->get_logger(),
"Parameter %s is deprecated. Please use the name %s instead.",
old_name.c_str(), current_name.c_str());
return value;
// set both parameters to the same value
nh->set_parameters({rclcpp::Parameter(current_name, old_name_value)});
current_name_value = old_name_value;
}
return default_value;
return current_name_value;
}

/**
Expand Down