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  post_set_params_handler_ = node->add_post_set_parameters_callback(
202  std::bind(
203  &ParameterHandler::updateParametersCallback,
204  this, std::placeholders::_1));
205  on_set_params_handler_ = node->add_on_set_parameters_callback(
206  std::bind(
207  &ParameterHandler::validateParameterUpdatesCallback,
208  this, std::placeholders::_1));
209 }
210 
211 ParameterHandler::~ParameterHandler()
212 {
213  auto node = node_.lock();
214  if (post_set_params_handler_ && node) {
215  node->remove_post_set_parameters_callback(post_set_params_handler_.get());
216  }
217  post_set_params_handler_.reset();
218  if (on_set_params_handler_ && node) {
219  node->remove_on_set_parameters_callback(on_set_params_handler_.get());
220  }
221  on_set_params_handler_.reset();
222 }
223 rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback(
224  std::vector<rclcpp::Parameter> parameters)
225 {
226  rcl_interfaces::msg::SetParametersResult result;
227  result.successful = true;
228  for (auto parameter : parameters) {
229  const auto & param_type = parameter.get_type();
230  const auto & param_name = parameter.get_name();
231  if (param_name.find(plugin_name_ + ".") != 0) {
232  continue;
233  }
234  if (param_type == ParameterType::PARAMETER_DOUBLE) {
235  if (param_name == plugin_name_ + ".inflation_cost_scaling_factor" &&
236  parameter.as_double() <= 0.0)
237  {
238  RCLCPP_WARN(
239  logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
240  "it should be >0. Ignoring parameter update.");
241  result.successful = false;
242  } else if (parameter.as_double() < 0.0) {
243  RCLCPP_WARN(
244  logger_, "The value of parameter '%s' is incorrectly set to %f, "
245  "it should be >=0. Ignoring parameter update.",
246  param_name.c_str(), parameter.as_double());
247  result.successful = false;
248  }
249  } else if (param_type == ParameterType::PARAMETER_BOOL) {
250  if (param_name == plugin_name_ + ".allow_reversing") {
251  if (params_.use_rotate_to_heading && parameter.as_bool()) {
252  RCLCPP_WARN(
253  logger_, "Both use_rotate_to_heading and allow_reversing "
254  "parameter cannot be set to true. Rejecting parameter update.");
255  result.successful = false;
256  }
257  }
258  }
259  }
260  return result;
261 }
262 void
263 ParameterHandler::updateParametersCallback(
264  std::vector<rclcpp::Parameter> parameters)
265 {
266  std::lock_guard<std::mutex> lock_reinit(mutex_);
267 
268  for (const auto & parameter : parameters) {
269  const auto & param_type = parameter.get_type();
270  const auto & param_name = parameter.get_name();
271 
272  if (param_type == ParameterType::PARAMETER_DOUBLE) {
273  if (param_name == plugin_name_ + ".inflation_cost_scaling_factor") {
274  params_.inflation_cost_scaling_factor = parameter.as_double();
275  } else if (param_name == plugin_name_ + ".desired_linear_vel") {
276  params_.desired_linear_vel = parameter.as_double();
277  params_.base_desired_linear_vel = parameter.as_double();
278  } else if (param_name == plugin_name_ + ".lookahead_dist") {
279  params_.lookahead_dist = parameter.as_double();
280  } else if (param_name == plugin_name_ + ".max_lookahead_dist") {
281  params_.max_lookahead_dist = parameter.as_double();
282  } else if (param_name == plugin_name_ + ".min_lookahead_dist") {
283  params_.min_lookahead_dist = parameter.as_double();
284  } else if (param_name == plugin_name_ + ".lookahead_time") {
285  params_.lookahead_time = parameter.as_double();
286  } else if (param_name == plugin_name_ + ".rotate_to_heading_angular_vel") {
287  params_.rotate_to_heading_angular_vel = parameter.as_double();
288  } else if (param_name == plugin_name_ + ".min_approach_linear_velocity") {
289  params_.min_approach_linear_velocity = parameter.as_double();
290  } else if (param_name == plugin_name_ + ".curvature_lookahead_dist") {
291  params_.curvature_lookahead_dist = parameter.as_double();
292  } else if (param_name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") {
293  params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double();
294  } else if (param_name == plugin_name_ + ".min_distance_to_obstacle") {
295  params_.min_distance_to_obstacle = parameter.as_double();
296  } else if (param_name == plugin_name_ + ".cost_scaling_dist") {
297  params_.cost_scaling_dist = parameter.as_double();
298  } else if (param_name == plugin_name_ + ".cost_scaling_gain") {
299  params_.cost_scaling_gain = parameter.as_double();
300  } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_radius") {
301  params_.regulated_linear_scaling_min_radius = parameter.as_double();
302  } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_speed") {
303  params_.regulated_linear_scaling_min_speed = parameter.as_double();
304  } else if (param_name == plugin_name_ + ".max_angular_accel") {
305  params_.max_angular_accel = parameter.as_double();
306  } else if (param_name == plugin_name_ + ".cancel_deceleration") {
307  params_.cancel_deceleration = parameter.as_double();
308  } else if (param_name == plugin_name_ + ".rotate_to_heading_min_angle") {
309  params_.rotate_to_heading_min_angle = parameter.as_double();
310  } else if (param_name == plugin_name_ + ".transform_tolerance") {
311  params_.transform_tolerance = parameter.as_double();
312  } else if (param_name == plugin_name_ + ".max_robot_pose_search_dist") {
313  params_.max_robot_pose_search_dist = parameter.as_double();
314  }
315  } else if (param_type == ParameterType::PARAMETER_BOOL) {
316  if (param_name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") {
317  params_.use_velocity_scaled_lookahead_dist = parameter.as_bool();
318  } else if (param_name == plugin_name_ + ".use_regulated_linear_velocity_scaling") {
319  params_.use_regulated_linear_velocity_scaling = parameter.as_bool();
320  } else if (param_name == plugin_name_ + ".use_fixed_curvature_lookahead") {
321  params_.use_fixed_curvature_lookahead = parameter.as_bool();
322  } else if (param_name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") {
323  params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool();
324  } else if (param_name == plugin_name_ + ".use_collision_detection") {
325  params_.use_collision_detection = parameter.as_bool();
326  } else if (param_name == plugin_name_ + ".stateful") {
327  params_.stateful = parameter.as_bool();
328  } else if (param_name == plugin_name_ + ".use_rotate_to_heading") {
329  params_.use_rotate_to_heading = parameter.as_bool();
330  } else if (param_name == plugin_name_ + ".use_cancel_deceleration") {
331  params_.use_cancel_deceleration = parameter.as_bool();
332  } else if (param_name == plugin_name_ + ".allow_reversing") {
333  params_.allow_reversing = parameter.as_bool();
334  } else if (param_name == plugin_name_ + ".interpolate_curvature_after_goal") {
335  params_.interpolate_curvature_after_goal = parameter.as_bool();
336  }
337  }
338  }
339 }
340 
341 } // 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:211
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::updateParametersCallback
void updateParametersCallback(std::vector< rclcpp::Parameter > parameters)
Callback executed when a parameter change is detected.
Definition: parameter_handler.cpp:263
Generated by doxygen 1.9.1