Skip to content

Commit 189b4b4

Browse files
sloretzSteveMacenski
sloretz
authored and
SteveMacenski
committed
Fix being unable to change StandardTrajectoryGenerator parameter vtheta_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>
1 parent f59812a commit 189b4b4

File tree

2 files changed

+70
-59
lines changed

2 files changed

+70
-59
lines changed

‎nav2_dwb_controller/dwb_plugins/test/twist_gen.cpp‎

Lines changed: 55 additions & 52 deletions
Original file line numberDiff line numberDiff line change
@@ -74,10 +74,14 @@ std::vector<rclcpp::Parameter> getDefaultKinematicParameters()
7474
return parameters;
7575
}
7676

77-
rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode(const std::string & name)
77+
rclcpp_lifecycle::LifecycleNode::SharedPtr makeTestNode(
78+
const std::string & name,
79+
const std::vector<rclcpp::Parameter> & overrides = {})
7880
{
79-
rclcpp::NodeOptions node_options = nav2_util::get_node_options_default();
81+
rclcpp::NodeOptions node_options;
8082
node_options.parameter_overrides(getDefaultKinematicParameters());
83+
node_options.parameter_overrides().insert(
84+
node_options.parameter_overrides().end(), overrides.begin(), overrides.end());
8185

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

143147
TEST(VelocityIterator, max_xy)
144148
{
145-
auto nh = makeTestNode("max_xy");
146-
nh->set_parameters({rclcpp::Parameter("dwb.max_speed_xy", 1.0)});
149+
auto nh = makeTestNode("max_xy", {rclcpp::Parameter("dwb.max_speed_xy", 1.0)});
147150
StandardTrajectoryGenerator gen;
148151
gen.initialize(nh, "dwb");
149152

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

156159
TEST(VelocityIterator, min_xy)
157160
{
158-
auto nh = makeTestNode("min_xy");
159-
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
161+
auto nh = makeTestNode("min_xy", {rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
160162
StandardTrajectoryGenerator gen;
161163
gen.initialize(nh, "dwb");
162164
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
@@ -167,8 +169,7 @@ TEST(VelocityIterator, min_xy)
167169

168170
TEST(VelocityIterator, min_theta)
169171
{
170-
auto nh = makeTestNode("min_theta");
171-
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
172+
auto nh = makeTestNode("min_theta", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
172173
StandardTrajectoryGenerator gen;
173174
gen.initialize(nh, "dwb");
174175
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
@@ -179,10 +180,11 @@ TEST(VelocityIterator, min_theta)
179180

180181
TEST(VelocityIterator, no_limits)
181182
{
182-
auto nh = makeTestNode("no_limits");
183-
nh->set_parameters({rclcpp::Parameter("dwb.max_speed_xy", -1.0)});
184-
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
185-
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
183+
auto nh = makeTestNode(
184+
"no_limits", {
185+
rclcpp::Parameter("dwb.max_speed_xy", -1.0),
186+
rclcpp::Parameter("dwb.min_speed_xy", -1.0),
187+
rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
186188
StandardTrajectoryGenerator gen;
187189
gen.initialize(nh, "dwb");
188190
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
@@ -193,14 +195,15 @@ TEST(VelocityIterator, no_limits)
193195

194196
TEST(VelocityIterator, no_limits_samples)
195197
{
196-
auto nh = makeTestNode("no_limits_samples");
197-
nh->set_parameters({rclcpp::Parameter("dwb.max_speed_xy", -1.0)});
198-
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
199-
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
200-
int x_samples = 10, y_samples = 3, theta_samples = 5;
201-
nh->set_parameters({rclcpp::Parameter("dwb.vx_samples", x_samples)});
202-
nh->set_parameters({rclcpp::Parameter("dwb.vy_samples", y_samples)});
203-
nh->set_parameters({rclcpp::Parameter("dwb.vtheta_samples", theta_samples)});
198+
const int x_samples = 10, y_samples = 3, theta_samples = 5;
199+
auto nh = makeTestNode(
200+
"no_limits_samples", {
201+
rclcpp::Parameter("dwb.max_speed_xy", -1.0),
202+
rclcpp::Parameter("dwb.min_speed_xy", -1.0),
203+
rclcpp::Parameter("dwb.min_speed_theta", -1.0),
204+
rclcpp::Parameter("dwb.vx_samples", x_samples),
205+
rclcpp::Parameter("dwb.vy_samples", y_samples),
206+
rclcpp::Parameter("dwb.vtheta_samples", theta_samples)});
204207
StandardTrajectoryGenerator gen;
205208
gen.initialize(nh, "dwb");
206209
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
@@ -210,8 +213,7 @@ TEST(VelocityIterator, no_limits_samples)
210213

211214
TEST(VelocityIterator, dwa_gen)
212215
{
213-
auto nh = makeTestNode("dwa_gen");
214-
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
216+
auto nh = makeTestNode("dwa_gen", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
215217
dwb_plugins::LimitedAccelGenerator gen;
216218
gen.initialize(nh, "dwb");
217219
std::vector<nav_2d_msgs::msg::Twist2D> twists = gen.getTwists(zero);
@@ -222,8 +224,7 @@ TEST(VelocityIterator, dwa_gen)
222224

223225
TEST(VelocityIterator, nonzero)
224226
{
225-
auto nh = makeTestNode("nonzero");
226-
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
227+
auto nh = makeTestNode("nonzero", {rclcpp::Parameter("dwb.min_speed_theta", -1.0)});
227228
dwb_plugins::LimitedAccelGenerator gen;
228229
gen.initialize(nh, "dwb");
229230
nav_2d_msgs::msg::Twist2D initial;
@@ -273,9 +274,8 @@ const double DEFAULT_SIM_TIME = 1.7;
273274

274275
TEST(TrajectoryGenerator, basic)
275276
{
276-
auto nh = makeTestNode("basic");
277+
auto nh = makeTestNode("basic", {rclcpp::Parameter("dwb.linear_granularity", 0.5)});
277278
StandardTrajectoryGenerator gen;
278-
nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)});
279279
gen.initialize(nh, "dwb");
280280
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
281281
matchTwist(res.velocity, forward);
@@ -290,10 +290,11 @@ TEST(TrajectoryGenerator, basic)
290290

291291
TEST(TrajectoryGenerator, basic_no_last_point)
292292
{
293-
auto nh = makeTestNode("basic_no_last_point");
293+
auto nh = makeTestNode(
294+
"basic_no_last_point", {
295+
rclcpp::Parameter("dwb.include_last_point", false),
296+
rclcpp::Parameter("dwb.linear_granularity", 0.5)});
294297
StandardTrajectoryGenerator gen;
295-
nh->set_parameters({rclcpp::Parameter("dwb.include_last_point", false)});
296-
nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)});
297298
gen.initialize(nh, "dwb");
298299
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
299300
matchTwist(res.velocity, forward);
@@ -308,9 +309,8 @@ TEST(TrajectoryGenerator, basic_no_last_point)
308309

309310
TEST(TrajectoryGenerator, too_slow)
310311
{
311-
auto nh = makeTestNode("too_slow");
312+
auto nh = makeTestNode("too_slow", {rclcpp::Parameter("dwb.linear_granularity", 0.5)});
312313
StandardTrajectoryGenerator gen;
313-
nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)});
314314
gen.initialize(nh, "dwb");
315315
nav_2d_msgs::msg::Twist2D cmd;
316316
cmd.x = 0.2;
@@ -326,9 +326,8 @@ TEST(TrajectoryGenerator, too_slow)
326326

327327
TEST(TrajectoryGenerator, holonomic)
328328
{
329-
auto nh = makeTestNode("holonomic");
329+
auto nh = makeTestNode("holonomic", {rclcpp::Parameter("dwb.linear_granularity", 0.5)});
330330
StandardTrajectoryGenerator gen;
331-
nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)});
332331
gen.initialize(nh, "dwb");
333332
nav_2d_msgs::msg::Twist2D cmd;
334333
cmd.x = 0.3;
@@ -346,10 +345,11 @@ TEST(TrajectoryGenerator, holonomic)
346345

347346
TEST(TrajectoryGenerator, twisty)
348347
{
349-
auto nh = makeTestNode("twisty");
348+
auto nh = makeTestNode(
349+
"twisty", {
350+
rclcpp::Parameter("dwb.linear_granularity", 0.5),
351+
rclcpp::Parameter("dwb.angular_granularity", 0.025)});
350352
StandardTrajectoryGenerator gen;
351-
nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)});
352-
nh->set_parameters({rclcpp::Parameter("dwb.angular_granularity", 0.025)});
353353
gen.initialize(nh, "dwb");
354354
nav_2d_msgs::msg::Twist2D cmd;
355355
cmd.x = 0.3;
@@ -370,10 +370,11 @@ TEST(TrajectoryGenerator, twisty)
370370

371371
TEST(TrajectoryGenerator, sim_time)
372372
{
373-
auto nh = makeTestNode("sim_time");
374373
const double sim_time = 2.5;
375-
nh->set_parameters({rclcpp::Parameter("dwb.sim_time", sim_time)});
376-
nh->set_parameters({rclcpp::Parameter("dwb.linear_granularity", 0.5)});
374+
auto nh = makeTestNode(
375+
"sim_time", {
376+
rclcpp::Parameter("dwb.sim_time", sim_time),
377+
rclcpp::Parameter("dwb.linear_granularity", 0.5)});
377378
StandardTrajectoryGenerator gen;
378379
gen.initialize(nh, "dwb");
379380
dwb_msgs::msg::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
@@ -389,12 +390,13 @@ TEST(TrajectoryGenerator, sim_time)
389390

390391
TEST(TrajectoryGenerator, accel)
391392
{
392-
auto nh = makeTestNode("accel");
393-
nh->set_parameters({rclcpp::Parameter("dwb.sim_time", 5.0)});
394-
nh->set_parameters({rclcpp::Parameter("dwb.discretize_by_time", true)});
395-
nh->set_parameters({rclcpp::Parameter("dwb.time_granularity", 1.0)});
396-
nh->set_parameters({rclcpp::Parameter("dwb.acc_lim_x", 0.1)});
397-
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
393+
auto nh = makeTestNode(
394+
"accel", {
395+
rclcpp::Parameter("dwb.sim_time", 5.0),
396+
rclcpp::Parameter("dwb.discretize_by_time", true),
397+
rclcpp::Parameter("dwb.time_granularity", 1.0),
398+
rclcpp::Parameter("dwb.acc_lim_x", 0.1),
399+
rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
398400
StandardTrajectoryGenerator gen;
399401
gen.initialize(nh, "dwb");
400402

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

413415
TEST(TrajectoryGenerator, dwa)
414416
{
415-
auto nh = makeTestNode("dwa");
416-
nh->set_parameters({rclcpp::Parameter("dwb.sim_period", 1.0)});
417-
nh->set_parameters({rclcpp::Parameter("dwb.sim_time", 5.0)});
418-
nh->set_parameters({rclcpp::Parameter("dwb.discretize_by_time", true)});
419-
nh->set_parameters({rclcpp::Parameter("dwb.time_granularity", 1.0)});
420-
nh->set_parameters({rclcpp::Parameter("dwb.acc_lim_x", 0.1)});
421-
nh->set_parameters({rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
417+
auto nh = makeTestNode(
418+
"dwa", {
419+
rclcpp::Parameter("dwb.sim_period", 1.0),
420+
rclcpp::Parameter("dwb.sim_time", 5.0),
421+
rclcpp::Parameter("dwb.discretize_by_time", true),
422+
rclcpp::Parameter("dwb.time_granularity", 1.0),
423+
rclcpp::Parameter("dwb.acc_lim_x", 0.1),
424+
rclcpp::Parameter("dwb.min_speed_xy", -1.0)});
422425
dwb_plugins::LimitedAccelGenerator gen;
423426
gen.initialize(nh, "dwb");
424427

‎nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/parameters.hpp‎

Lines changed: 15 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -93,18 +93,26 @@ param_t loadParameterWithDeprecation(
9393
const nav2_util::LifecycleNode::SharedPtr & nh, const std::string current_name,
9494
const std::string old_name, const param_t & default_value)
9595
{
96-
param_t value = 0;
97-
if (nh->get_parameter(current_name, value)) {
98-
return value;
99-
}
100-
if (nh->get_parameter(old_name, value)) {
96+
nav2_util::declare_parameter_if_not_declared(
97+
nh, current_name, rclcpp::ParameterValue(default_value));
98+
nav2_util::declare_parameter_if_not_declared(
99+
nh, old_name, rclcpp::ParameterValue(default_value));
100+
101+
param_t current_name_value;
102+
nh->get_parameter(current_name, current_name_value);
103+
param_t old_name_value;
104+
nh->get_parameter(old_name, old_name_value);
105+
106+
if (old_name_value != current_name_value && old_name_value != default_value) {
101107
RCLCPP_WARN(
102108
nh->get_logger(),
103109
"Parameter %s is deprecated. Please use the name %s instead.",
104110
old_name.c_str(), current_name.c_str());
105-
return value;
111+
// set both parameters to the same value
112+
nh->set_parameters({rclcpp::Parameter(current_name, old_name_value)});
113+
current_name_value = old_name_value;
106114
}
107-
return default_value;
115+
return current_name_value;
108116
}
109117

110118
/**

0 commit comments

Comments
 (0)