From bd278bdc7b8f5671a64ca6cb9ac3631b17c3601e Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 30 Mar 2020 21:10:16 -0700 Subject: [PATCH 1/3] Fix tests declaring parameters real nodes don't Signed-off-by: Shane Loretz --- .../dwb_plugins/test/twist_gen.cpp | 76 +++++++++---------- 1 file changed, 37 insertions(+), 39 deletions(-) diff --git a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp index 26a5e052f0e..0e16553dc0c 100644 --- a/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp +++ b/nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp @@ -74,10 +74,14 @@ std::vector getDefaultKinematicParameters() return parameters; } -rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode(const std::string & name) +rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode( + const std::string & name, + const std::vector & 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()); @@ -142,8 +146,7 @@ TEST(VelocityIterator, standard_gen) TEST(VelocityIterator, max_xy) { - auto nh = makeTestNode("max_xy"); - nh->set_parameters({rclcpp::Parameter("max_speed_xy", 1.0)}); + auto nh = makeTestNode("max_xy", {rclcpp::Parameter("max_speed_xy", 1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh); @@ -155,8 +158,7 @@ TEST(VelocityIterator, max_xy) TEST(VelocityIterator, min_xy) { - auto nh = makeTestNode("min_xy"); - nh->set_parameters({rclcpp::Parameter("min_speed_xy", -1.0)}); + auto nh = makeTestNode("min_xy", {rclcpp::Parameter("min_speed_xy", -1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh); std::vector twists = gen.getTwists(zero); @@ -167,8 +169,7 @@ TEST(VelocityIterator, min_xy) TEST(VelocityIterator, min_theta) { - auto nh = makeTestNode("min_theta"); - nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)}); + auto nh = makeTestNode("min_theta", {rclcpp::Parameter("min_speed_theta", -1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh); std::vector twists = gen.getTwists(zero); @@ -179,10 +180,10 @@ TEST(VelocityIterator, min_theta) TEST(VelocityIterator, no_limits) { - auto nh = makeTestNode("no_limits"); - nh->set_parameters({rclcpp::Parameter("max_speed_xy", -1.0)}); - nh->set_parameters({rclcpp::Parameter("min_speed_xy", -1.0)}); - nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)}); + auto nh = makeTestNode("no_limits", { + rclcpp::Parameter("max_speed_xy", -1.0), + rclcpp::Parameter("min_speed_xy", -1.0), + rclcpp::Parameter("min_speed_theta", -1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh); std::vector twists = gen.getTwists(zero); @@ -193,14 +194,14 @@ TEST(VelocityIterator, no_limits) TEST(VelocityIterator, no_limits_samples) { - auto nh = makeTestNode("no_limits_samples"); - nh->set_parameters({rclcpp::Parameter("max_speed_xy", -1.0)}); - nh->set_parameters({rclcpp::Parameter("min_speed_xy", -1.0)}); - nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)}); - int x_samples = 10, y_samples = 3, theta_samples = 5; - nh->set_parameters({rclcpp::Parameter("vx_samples", x_samples)}); - nh->set_parameters({rclcpp::Parameter("vy_samples", y_samples)}); - nh->set_parameters({rclcpp::Parameter("vtheta_samples", theta_samples)}); + const int x_samples = 10, y_samples = 3, theta_samples = 5; + auto nh = makeTestNode("no_limits_samples", { + rclcpp::Parameter("max_speed_xy", -1.0), + rclcpp::Parameter("min_speed_xy", -1.0), + rclcpp::Parameter("min_speed_theta", -1.0), + rclcpp::Parameter("vx_samples", x_samples), + rclcpp::Parameter("vy_samples", y_samples), + rclcpp::Parameter("vtheta_samples", theta_samples)}); StandardTrajectoryGenerator gen; gen.initialize(nh); std::vector twists = gen.getTwists(zero); @@ -210,8 +211,7 @@ TEST(VelocityIterator, no_limits_samples) TEST(VelocityIterator, dwa_gen) { - auto nh = makeTestNode("dwa_gen"); - nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)}); + auto nh = makeTestNode("dwa_gen", {rclcpp::Parameter("min_speed_theta", -1.0)}); dwb_plugins::LimitedAccelGenerator gen; gen.initialize(nh); std::vector twists = gen.getTwists(zero); @@ -222,8 +222,7 @@ TEST(VelocityIterator, dwa_gen) TEST(VelocityIterator, nonzero) { - auto nh = makeTestNode("nonzero"); - nh->set_parameters({rclcpp::Parameter("min_speed_theta", -1.0)}); + auto nh = makeTestNode("nonzero", {rclcpp::Parameter("min_speed_theta", -1.0)}); dwb_plugins::LimitedAccelGenerator gen; gen.initialize(nh); nav_2d_msgs::msg::Twist2D initial; @@ -342,8 +341,7 @@ TEST(TrajectoryGenerator, twisty) TEST(TrajectoryGenerator, sim_time) { - auto nh = makeTestNode("sim_time"); - nh->set_parameters({rclcpp::Parameter("sim_time", 2.5)}); + auto nh = makeTestNode("sim_time", {rclcpp::Parameter("sim_time", 2.5)}); StandardTrajectoryGenerator gen; gen.initialize(nh); dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward); @@ -359,12 +357,12 @@ TEST(TrajectoryGenerator, sim_time) TEST(TrajectoryGenerator, accel) { - auto nh = makeTestNode("accel"); - nh->set_parameters({rclcpp::Parameter("sim_time", 5.0)}); - nh->set_parameters({rclcpp::Parameter("discretize_by_time", true)}); - nh->set_parameters({rclcpp::Parameter("sim_granularity", 1.0)}); - nh->set_parameters({rclcpp::Parameter("acc_lim_x", 0.1)}); - nh->set_parameters({rclcpp::Parameter("min_speed_xy", -1.0)}); + auto nh = makeTestNode("accel", { + rclcpp::Parameter("sim_time", 5.0), + rclcpp::Parameter("discretize_by_time", true), + rclcpp::Parameter("sim_granularity", 1.0), + rclcpp::Parameter("acc_lim_x", 0.1), + rclcpp::Parameter("min_speed_xy", -1.0)}); StandardTrajectoryGenerator gen; gen.initialize(nh); @@ -381,13 +379,13 @@ TEST(TrajectoryGenerator, accel) TEST(TrajectoryGenerator, dwa) { - auto nh = makeTestNode("dwa"); - nh->set_parameters({rclcpp::Parameter("sim_period", 1.0)}); - nh->set_parameters({rclcpp::Parameter("sim_time", 5.0)}); - nh->set_parameters({rclcpp::Parameter("discretize_by_time", true)}); - nh->set_parameters({rclcpp::Parameter("sim_granularity", 1.0)}); - nh->set_parameters({rclcpp::Parameter("acc_lim_x", 0.1)}); - nh->set_parameters({rclcpp::Parameter("min_speed_xy", -1.0)}); + auto nh = makeTestNode("dwa", { + rclcpp::Parameter("sim_period", 1.0), + rclcpp::Parameter("sim_time", 5.0), + rclcpp::Parameter("discretize_by_time", true), + rclcpp::Parameter("sim_granularity", 1.0), + rclcpp::Parameter("acc_lim_x", 0.1), + rclcpp::Parameter("min_speed_xy", -1.0)}); dwb_plugins::LimitedAccelGenerator gen; gen.initialize(nh); From 4dba91ee7a544e146308b261020bdfcae785522f Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 30 Mar 2020 21:10:42 -0700 Subject: [PATCH 2/3] Fix loadParameterWithDeprecation not getting initial parameter values Signed-off-by: Shane Loretz --- .../include/nav_2d_utils/parameters.hpp | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp index 50263460c1e..b10d6432154 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp @@ -92,17 +92,23 @@ 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; } /** From d7110880ab31ed69901c325c054f77374c2dd7e7 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 31 Mar 2020 09:23:53 -0700 Subject: [PATCH 3/3] Line length < 100 Signed-off-by: Shane Loretz --- .../nav_2d_utils/include/nav_2d_utils/parameters.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp index b10d6432154..63fe9cd9ec1 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp @@ -92,8 +92,10 @@ param_t loadParameterWithDeprecation( const nav2_util::LifecycleNode::SharedPtr & nh, const std::string current_name, const std::string old_name, const param_t & default_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)); + 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);