Skip to content

Commit

Permalink
Fix being unable to change StandardTrajectoryGenerator parameter vthe…
Browse files Browse the repository at this point in the history 
…ta_samples (#1619)

* Fix tests declaring parameters real nodes don't

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Fix loadParameterWithDeprecation not getting initial parameter values

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Create sim_time variable before using it

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Line length < 100

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Add missing {

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

* Linter fixes

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* sim_granularity -> time_granularity

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Linter fix

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>
  • Loading branch information
@sloretz
sloretz authored Aug 21, 2020
1 parent b6b17e2 commit d191e0a
Show file tree
Hide file tree
Showing 2 changed files with 70 additions and 59 deletions.
107 changes: 55 additions & 52 deletions nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp
View file
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
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

0 comments on commit d191e0a

Please sign in to comment.