Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
parameter_handler.cpp
1 // Copyright (c) 2022 Samsung Research America
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 
15 #include <algorithm>
16 #include <string>
17 #include <limits>
18 #include <memory>
19 #include <vector>
20 #include <utility>
21 
22 #include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp"
23 
24 namespace nav2_regulated_pure_pursuit_controller
25 {
26 
27 using nav2::declare_parameter_if_not_declared;
28 using rcl_interfaces::msg::ParameterType;
29 
30 ParameterHandler::ParameterHandler(
31  nav2::LifecycleNode::SharedPtr node,
32  std::string & plugin_name, rclcpp::Logger & logger,
33  const double costmap_size_x)
34 {
35  node_ = node;
36  plugin_name_ = plugin_name;
37  logger_ = logger;
38 
39  declare_parameter_if_not_declared(
40  node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5));
41  declare_parameter_if_not_declared(
42  node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6));
43  declare_parameter_if_not_declared(
44  node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3));
45  declare_parameter_if_not_declared(
46  node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9));
47  declare_parameter_if_not_declared(
48  node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5));
49  declare_parameter_if_not_declared(
50  node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8));
51  declare_parameter_if_not_declared(
52  node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1));
53  declare_parameter_if_not_declared(
54  node, plugin_name_ + ".use_velocity_scaled_lookahead_dist",
55  rclcpp::ParameterValue(false));
56  declare_parameter_if_not_declared(
57  node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05));
58  declare_parameter_if_not_declared(
59  node, plugin_name_ + ".approach_velocity_scaling_dist",
60  rclcpp::ParameterValue(0.6));
61  declare_parameter_if_not_declared(
62  node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot",
63  rclcpp::ParameterValue(1.0));
64  declare_parameter_if_not_declared(
65  node, plugin_name_ + ".min_distance_to_obstacle",
66  rclcpp::ParameterValue(-1.0));
67  declare_parameter_if_not_declared(
68  node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true));
69  declare_parameter_if_not_declared(
70  node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling",
71  rclcpp::ParameterValue(true));
72  declare_parameter_if_not_declared(
73  node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6));
74  declare_parameter_if_not_declared(
75  node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0));
76  declare_parameter_if_not_declared(
77  node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0));
78  declare_parameter_if_not_declared(
79  node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90));
80  declare_parameter_if_not_declared(
81  node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25));
82  declare_parameter_if_not_declared(
83  node, plugin_name_ + ".use_fixed_curvature_lookahead", rclcpp::ParameterValue(false));
84  declare_parameter_if_not_declared(
85  node, plugin_name_ + ".curvature_lookahead_dist", rclcpp::ParameterValue(0.6));
86  declare_parameter_if_not_declared(
87  node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true));
88  declare_parameter_if_not_declared(
89  node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785));
90  declare_parameter_if_not_declared(
91  node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
92  declare_parameter_if_not_declared(
93  node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false));
94  declare_parameter_if_not_declared(
95  node, plugin_name_ + ".cancel_deceleration", rclcpp::ParameterValue(3.2));
96  declare_parameter_if_not_declared(
97  node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false));
98  declare_parameter_if_not_declared(
99  node, plugin_name_ + ".max_robot_pose_search_dist",
100  rclcpp::ParameterValue(costmap_size_x / 2.0));
101  declare_parameter_if_not_declared(
102  node, plugin_name_ + ".interpolate_curvature_after_goal",
103  rclcpp::ParameterValue(false));
104  declare_parameter_if_not_declared(
105  node, plugin_name_ + ".use_collision_detection",
106  rclcpp::ParameterValue(true));
107  declare_parameter_if_not_declared(
108  node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true));
109 
110  node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel);
111  params_.base_desired_linear_vel = params_.desired_linear_vel;
112  node->get_parameter(plugin_name_ + ".lookahead_dist", params_.lookahead_dist);
113  node->get_parameter(plugin_name_ + ".min_lookahead_dist", params_.min_lookahead_dist);
114  node->get_parameter(plugin_name_ + ".max_lookahead_dist", params_.max_lookahead_dist);
115  node->get_parameter(plugin_name_ + ".lookahead_time", params_.lookahead_time);
116  node->get_parameter(
117  plugin_name_ + ".rotate_to_heading_angular_vel",
118  params_.rotate_to_heading_angular_vel);
119  node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance);
120  node->get_parameter(
121  plugin_name_ + ".use_velocity_scaled_lookahead_dist",
122  params_.use_velocity_scaled_lookahead_dist);
123  node->get_parameter(
124  plugin_name_ + ".min_approach_linear_velocity",
125  params_.min_approach_linear_velocity);
126  node->get_parameter(
127  plugin_name_ + ".approach_velocity_scaling_dist",
128  params_.approach_velocity_scaling_dist);
129  if (params_.approach_velocity_scaling_dist > costmap_size_x / 2.0) {
130  RCLCPP_WARN(
131  logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, "
132  "leading to permanent slowdown");
133  }
134  node->get_parameter(
135  plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot",
136  params_.max_allowed_time_to_collision_up_to_carrot);
137  node->get_parameter(
138  plugin_name_ + ".min_distance_to_obstacle",
139  params_.min_distance_to_obstacle);
140  node->get_parameter(
141  plugin_name_ + ".use_regulated_linear_velocity_scaling",
142  params_.use_regulated_linear_velocity_scaling);
143  node->get_parameter(
144  plugin_name_ + ".use_cost_regulated_linear_velocity_scaling",
145  params_.use_cost_regulated_linear_velocity_scaling);
146  node->get_parameter(plugin_name_ + ".cost_scaling_dist", params_.cost_scaling_dist);
147  node->get_parameter(plugin_name_ + ".cost_scaling_gain", params_.cost_scaling_gain);
148  node->get_parameter(
149  plugin_name_ + ".inflation_cost_scaling_factor",
150  params_.inflation_cost_scaling_factor);
151  node->get_parameter(
152  plugin_name_ + ".regulated_linear_scaling_min_radius",
153  params_.regulated_linear_scaling_min_radius);
154  node->get_parameter(
155  plugin_name_ + ".regulated_linear_scaling_min_speed",
156  params_.regulated_linear_scaling_min_speed);
157  node->get_parameter(
158  plugin_name_ + ".use_fixed_curvature_lookahead",
159  params_.use_fixed_curvature_lookahead);
160  node->get_parameter(
161  plugin_name_ + ".curvature_lookahead_dist",
162  params_.curvature_lookahead_dist);
163  node->get_parameter(plugin_name_ + ".use_rotate_to_heading", params_.use_rotate_to_heading);
164  node->get_parameter(
165  plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle);
166  node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel);
167  node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration);
168  node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration);
169  node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing);
170  node->get_parameter(
171  plugin_name_ + ".max_robot_pose_search_dist",
172  params_.max_robot_pose_search_dist);
173  if (params_.max_robot_pose_search_dist < 0.0) {
174  RCLCPP_WARN(
175  logger_, "Max robot search distance is negative, setting to max to search"
176  " every point on path for the closest value.");
177  params_.max_robot_pose_search_dist = std::numeric_limits<double>::max();
178  }
179 
180  node->get_parameter(
181  plugin_name_ + ".interpolate_curvature_after_goal",
182  params_.interpolate_curvature_after_goal);
183  if (!params_.use_fixed_curvature_lookahead && params_.interpolate_curvature_after_goal) {
184  RCLCPP_WARN(
185  logger_, "For interpolate_curvature_after_goal to be set to true, "
186  "use_fixed_curvature_lookahead should be true, it is currently set to false. Disabling.");
187  params_.interpolate_curvature_after_goal = false;
188  }
189  node->get_parameter(
190  plugin_name_ + ".use_collision_detection",
191  params_.use_collision_detection);
192  node->get_parameter(plugin_name_ + ".stateful", params_.stateful);
193 
194  if (params_.inflation_cost_scaling_factor <= 0.0) {
195  RCLCPP_WARN(
196  logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
197  "it should be >0. Disabling cost regulated linear velocity scaling.");
198  params_.use_cost_regulated_linear_velocity_scaling = false;
199  }
200 }
201 
202 void ParameterHandler::activate()
203 {
204  auto node = node_.lock();
205  post_set_params_handler_ = node->add_post_set_parameters_callback(
206  std::bind(
207  &ParameterHandler::updateParametersCallback,
208  this, std::placeholders::_1));
209  on_set_params_handler_ = node->add_on_set_parameters_callback(
210  std::bind(
211  &ParameterHandler::validateParameterUpdatesCallback,
212  this, std::placeholders::_1));
213 }
214 
215 void ParameterHandler::deactivate()
216 {
217  auto node = node_.lock();
218  if (post_set_params_handler_ && node) {
219  node->remove_post_set_parameters_callback(post_set_params_handler_.get());
220  }
221  post_set_params_handler_.reset();
222  if (on_set_params_handler_ && node) {
223  node->remove_on_set_parameters_callback(on_set_params_handler_.get());
224  }
225  on_set_params_handler_.reset();
226 }
227 
228 ParameterHandler::~ParameterHandler()
229 {
230 }
231 
232 rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback(
233  std::vector<rclcpp::Parameter> parameters)
234 {
235  rcl_interfaces::msg::SetParametersResult result;
236  result.successful = true;
237  for (auto parameter : parameters) {
238  const auto & param_type = parameter.get_type();
239  const auto & param_name = parameter.get_name();
240  if (param_name.find(plugin_name_ + ".") != 0) {
241  continue;
242  }
243  if (param_type == ParameterType::PARAMETER_DOUBLE) {
244  if (param_name == plugin_name_ + ".inflation_cost_scaling_factor" &&
245  parameter.as_double() <= 0.0)
246  {
247  RCLCPP_WARN(
248  logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
249  "it should be >0. Ignoring parameter update.");
250  result.successful = false;
251  } else if (parameter.as_double() < 0.0) {
252  RCLCPP_WARN(
253  logger_, "The value of parameter '%s' is incorrectly set to %f, "
254  "it should be >=0. Ignoring parameter update.",
255  param_name.c_str(), parameter.as_double());
256  result.successful = false;
257  }
258  } else if (param_type == ParameterType::PARAMETER_BOOL) {
259  if (param_name == plugin_name_ + ".allow_reversing") {
260  if (params_.use_rotate_to_heading && parameter.as_bool()) {
261  RCLCPP_WARN(
262  logger_, "Both use_rotate_to_heading and allow_reversing "
263  "parameter cannot be set to true. Rejecting parameter update.");
264  result.successful = false;
265  }
266  }
267  }
268  }
269  return result;
270 }
271 void
272 ParameterHandler::updateParametersCallback(
273  std::vector<rclcpp::Parameter> parameters)
274 {
275  std::lock_guard<std::mutex> lock_reinit(mutex_);
276 
277  for (const auto & parameter : parameters) {
278  const auto & param_type = parameter.get_type();
279  const auto & param_name = parameter.get_name();
280  if (param_name.find(plugin_name_ + ".") != 0) {
281  continue;
282  }
283  if (param_type == ParameterType::PARAMETER_DOUBLE) {
284  if (param_name == plugin_name_ + ".inflation_cost_scaling_factor") {
285  params_.inflation_cost_scaling_factor = parameter.as_double();
286  } else if (param_name == plugin_name_ + ".desired_linear_vel") {
287  params_.desired_linear_vel = parameter.as_double();
288  params_.base_desired_linear_vel = parameter.as_double();
289  } else if (param_name == plugin_name_ + ".lookahead_dist") {
290  params_.lookahead_dist = parameter.as_double();
291  } else if (param_name == plugin_name_ + ".max_lookahead_dist") {
292  params_.max_lookahead_dist = parameter.as_double();
293  } else if (param_name == plugin_name_ + ".min_lookahead_dist") {
294  params_.min_lookahead_dist = parameter.as_double();
295  } else if (param_name == plugin_name_ + ".lookahead_time") {
296  params_.lookahead_time = parameter.as_double();
297  } else if (param_name == plugin_name_ + ".rotate_to_heading_angular_vel") {
298  params_.rotate_to_heading_angular_vel = parameter.as_double();
299  } else if (param_name == plugin_name_ + ".min_approach_linear_velocity") {
300  params_.min_approach_linear_velocity = parameter.as_double();
301  } else if (param_name == plugin_name_ + ".curvature_lookahead_dist") {
302  params_.curvature_lookahead_dist = parameter.as_double();
303  } else if (param_name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") {
304  params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double();
305  } else if (param_name == plugin_name_ + ".min_distance_to_obstacle") {
306  params_.min_distance_to_obstacle = parameter.as_double();
307  } else if (param_name == plugin_name_ + ".cost_scaling_dist") {
308  params_.cost_scaling_dist = parameter.as_double();
309  } else if (param_name == plugin_name_ + ".cost_scaling_gain") {
310  params_.cost_scaling_gain = parameter.as_double();
311  } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_radius") {
312  params_.regulated_linear_scaling_min_radius = parameter.as_double();
313  } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_speed") {
314  params_.regulated_linear_scaling_min_speed = parameter.as_double();
315  } else if (param_name == plugin_name_ + ".max_angular_accel") {
316  params_.max_angular_accel = parameter.as_double();
317  } else if (param_name == plugin_name_ + ".cancel_deceleration") {
318  params_.cancel_deceleration = parameter.as_double();
319  } else if (param_name == plugin_name_ + ".rotate_to_heading_min_angle") {
320  params_.rotate_to_heading_min_angle = parameter.as_double();
321  } else if (param_name == plugin_name_ + ".transform_tolerance") {
322  params_.transform_tolerance = parameter.as_double();
323  } else if (param_name == plugin_name_ + ".max_robot_pose_search_dist") {
324  params_.max_robot_pose_search_dist = parameter.as_double();
325  }
326  } else if (param_type == ParameterType::PARAMETER_BOOL) {
327  if (param_name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") {
328  params_.use_velocity_scaled_lookahead_dist = parameter.as_bool();
329  } else if (param_name == plugin_name_ + ".use_regulated_linear_velocity_scaling") {
330  params_.use_regulated_linear_velocity_scaling = parameter.as_bool();
331  } else if (param_name == plugin_name_ + ".use_fixed_curvature_lookahead") {
332  params_.use_fixed_curvature_lookahead = parameter.as_bool();
333  } else if (param_name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") {
334  params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool();
335  } else if (param_name == plugin_name_ + ".use_collision_detection") {
336  params_.use_collision_detection = parameter.as_bool();
337  } else if (param_name == plugin_name_ + ".stateful") {
338  params_.stateful = parameter.as_bool();
339  } else if (param_name == plugin_name_ + ".use_rotate_to_heading") {
340  params_.use_rotate_to_heading = parameter.as_bool();
341  } else if (param_name == plugin_name_ + ".use_cancel_deceleration") {
342  params_.use_cancel_deceleration = parameter.as_bool();
343  } else if (param_name == plugin_name_ + ".allow_reversing") {
344  params_.allow_reversing = parameter.as_bool();
345  } else if (param_name == plugin_name_ + ".interpolate_curvature_after_goal") {
346  params_.interpolate_curvature_after_goal = parameter.as_bool();
347  }
348  }
349  }
350 }
351 
352 } // namespace nav2_regulated_pure_pursuit_controller
nav2_regulated_pure_pursuit_controller::ParameterHandler::~ParameterHandler
~ParameterHandler()
Destrructor for nav2_regulated_pure_pursuit_controller::ParameterHandler.
Definition: parameter_handler.cpp:228
nav2_regulated_pure_pursuit_controller::ParameterHandler::activate
void activate()
Registers callbacks for dynamic parameter handling.
Definition: parameter_handler.cpp:202
nav2_regulated_pure_pursuit_controller::ParameterHandler::validateParameterUpdatesCallback
rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(std::vector< rclcpp::Parameter > parameters)
Validate incoming parameter updates before applying them. This callback is triggered when one or more...
Definition: parameter_handler.cpp:232
nav2_regulated_pure_pursuit_controller::ParameterHandler::ParameterHandler
ParameterHandler(nav2::LifecycleNode::SharedPtr node, std::string &plugin_name, rclcpp::Logger &logger, const double costmap_size_x)
Constructor for nav2_regulated_pure_pursuit_controller::ParameterHandler.
Definition: parameter_handler.cpp:30
nav2_regulated_pure_pursuit_controller::ParameterHandler::deactivate
void deactivate()
Resets callbacks for dynamic parameter handling.
Definition: parameter_handler.cpp:215
nav2_regulated_pure_pursuit_controller::ParameterHandler::updateParametersCallback
void updateParametersCallback(std::vector< rclcpp::Parameter > parameters)
Apply parameter updates after validation This callback is executed when parameters have been successf...
Definition: parameter_handler.cpp:272
Generated by doxygen 1.9.1