Nav2 Navigation Stack - kilted  kilted
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_util::declare_parameter_if_not_declared;
28 using rcl_interfaces::msg::ParameterType;
29 
30 ParameterHandler::ParameterHandler(
31  rclcpp_lifecycle::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_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true));
66  declare_parameter_if_not_declared(
67  node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling",
68  rclcpp::ParameterValue(true));
69  declare_parameter_if_not_declared(
70  node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6));
71  declare_parameter_if_not_declared(
72  node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0));
73  declare_parameter_if_not_declared(
74  node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0));
75  declare_parameter_if_not_declared(
76  node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90));
77  declare_parameter_if_not_declared(
78  node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25));
79  declare_parameter_if_not_declared(
80  node, plugin_name_ + ".use_fixed_curvature_lookahead", rclcpp::ParameterValue(false));
81  declare_parameter_if_not_declared(
82  node, plugin_name_ + ".curvature_lookahead_dist", rclcpp::ParameterValue(0.6));
83  declare_parameter_if_not_declared(
84  node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true));
85  declare_parameter_if_not_declared(
86  node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785));
87  declare_parameter_if_not_declared(
88  node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
89  declare_parameter_if_not_declared(
90  node, plugin_name_ + ".use_cancel_deceleration", rclcpp::ParameterValue(false));
91  declare_parameter_if_not_declared(
92  node, plugin_name_ + ".cancel_deceleration", rclcpp::ParameterValue(3.2));
93  declare_parameter_if_not_declared(
94  node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false));
95  declare_parameter_if_not_declared(
96  node, plugin_name_ + ".max_robot_pose_search_dist",
97  rclcpp::ParameterValue(costmap_size_x / 2.0));
98  declare_parameter_if_not_declared(
99  node, plugin_name_ + ".interpolate_curvature_after_goal",
100  rclcpp::ParameterValue(false));
101  declare_parameter_if_not_declared(
102  node, plugin_name_ + ".use_collision_detection",
103  rclcpp::ParameterValue(true));
104  declare_parameter_if_not_declared(
105  node, plugin_name_ + ".stateful", rclcpp::ParameterValue(true));
106 
107  node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel);
108  params_.base_desired_linear_vel = params_.desired_linear_vel;
109  node->get_parameter(plugin_name_ + ".lookahead_dist", params_.lookahead_dist);
110  node->get_parameter(plugin_name_ + ".min_lookahead_dist", params_.min_lookahead_dist);
111  node->get_parameter(plugin_name_ + ".max_lookahead_dist", params_.max_lookahead_dist);
112  node->get_parameter(plugin_name_ + ".lookahead_time", params_.lookahead_time);
113  node->get_parameter(
114  plugin_name_ + ".rotate_to_heading_angular_vel",
115  params_.rotate_to_heading_angular_vel);
116  node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance);
117  node->get_parameter(
118  plugin_name_ + ".use_velocity_scaled_lookahead_dist",
119  params_.use_velocity_scaled_lookahead_dist);
120  node->get_parameter(
121  plugin_name_ + ".min_approach_linear_velocity",
122  params_.min_approach_linear_velocity);
123  node->get_parameter(
124  plugin_name_ + ".approach_velocity_scaling_dist",
125  params_.approach_velocity_scaling_dist);
126  if (params_.approach_velocity_scaling_dist > costmap_size_x / 2.0) {
127  RCLCPP_WARN(
128  logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, "
129  "leading to permanent slowdown");
130  }
131  node->get_parameter(
132  plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot",
133  params_.max_allowed_time_to_collision_up_to_carrot);
134  node->get_parameter(
135  plugin_name_ + ".use_regulated_linear_velocity_scaling",
136  params_.use_regulated_linear_velocity_scaling);
137  node->get_parameter(
138  plugin_name_ + ".use_cost_regulated_linear_velocity_scaling",
139  params_.use_cost_regulated_linear_velocity_scaling);
140  node->get_parameter(plugin_name_ + ".cost_scaling_dist", params_.cost_scaling_dist);
141  node->get_parameter(plugin_name_ + ".cost_scaling_gain", params_.cost_scaling_gain);
142  node->get_parameter(
143  plugin_name_ + ".inflation_cost_scaling_factor",
144  params_.inflation_cost_scaling_factor);
145  node->get_parameter(
146  plugin_name_ + ".regulated_linear_scaling_min_radius",
147  params_.regulated_linear_scaling_min_radius);
148  node->get_parameter(
149  plugin_name_ + ".regulated_linear_scaling_min_speed",
150  params_.regulated_linear_scaling_min_speed);
151  node->get_parameter(
152  plugin_name_ + ".use_fixed_curvature_lookahead",
153  params_.use_fixed_curvature_lookahead);
154  node->get_parameter(
155  plugin_name_ + ".curvature_lookahead_dist",
156  params_.curvature_lookahead_dist);
157  node->get_parameter(plugin_name_ + ".use_rotate_to_heading", params_.use_rotate_to_heading);
158  node->get_parameter(
159  plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle);
160  node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel);
161  node->get_parameter(plugin_name_ + ".use_cancel_deceleration", params_.use_cancel_deceleration);
162  node->get_parameter(plugin_name_ + ".cancel_deceleration", params_.cancel_deceleration);
163  node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing);
164  node->get_parameter(
165  plugin_name_ + ".max_robot_pose_search_dist",
166  params_.max_robot_pose_search_dist);
167  if (params_.max_robot_pose_search_dist < 0.0) {
168  RCLCPP_WARN(
169  logger_, "Max robot search distance is negative, setting to max to search"
170  " every point on path for the closest value.");
171  params_.max_robot_pose_search_dist = std::numeric_limits<double>::max();
172  }
173 
174  node->get_parameter(
175  plugin_name_ + ".interpolate_curvature_after_goal",
176  params_.interpolate_curvature_after_goal);
177  if (!params_.use_fixed_curvature_lookahead && params_.interpolate_curvature_after_goal) {
178  RCLCPP_WARN(
179  logger_, "For interpolate_curvature_after_goal to be set to true, "
180  "use_fixed_curvature_lookahead should be true, it is currently set to false. Disabling.");
181  params_.interpolate_curvature_after_goal = false;
182  }
183  node->get_parameter(
184  plugin_name_ + ".use_collision_detection",
185  params_.use_collision_detection);
186  node->get_parameter(plugin_name_ + ".stateful", params_.stateful);
187 
188  if (params_.inflation_cost_scaling_factor <= 0.0) {
189  RCLCPP_WARN(
190  logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
191  "it should be >0. Disabling cost regulated linear velocity scaling.");
192  params_.use_cost_regulated_linear_velocity_scaling = false;
193  }
194 
195  post_set_params_handler_ = node->add_post_set_parameters_callback(
196  std::bind(
197  &ParameterHandler::updateParametersCallback,
198  this, std::placeholders::_1));
199  on_set_params_handler_ = node->add_on_set_parameters_callback(
200  std::bind(
201  &ParameterHandler::validateParameterUpdatesCallback,
202  this, std::placeholders::_1));
203 }
204 
205 ParameterHandler::~ParameterHandler()
206 {
207  auto node = node_.lock();
208  if (post_set_params_handler_ && node) {
209  node->remove_post_set_parameters_callback(post_set_params_handler_.get());
210  }
211  post_set_params_handler_.reset();
212  if (on_set_params_handler_ && node) {
213  node->remove_on_set_parameters_callback(on_set_params_handler_.get());
214  }
215  on_set_params_handler_.reset();
216 }
217 rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback(
218  std::vector<rclcpp::Parameter> parameters)
219 {
220  rcl_interfaces::msg::SetParametersResult result;
221  result.successful = true;
222  for (auto parameter : parameters) {
223  const auto & param_type = parameter.get_type();
224  const auto & param_name = parameter.get_name();
225  if (param_name.find(plugin_name_ + ".") != 0) {
226  continue;
227  }
228  if (param_type == ParameterType::PARAMETER_DOUBLE) {
229  if (param_name == plugin_name_ + ".inflation_cost_scaling_factor" &&
230  parameter.as_double() <= 0.0)
231  {
232  RCLCPP_WARN(
233  logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
234  "it should be >0. Ignoring parameter update.");
235  result.successful = false;
236  } else if (parameter.as_double() < 0.0) {
237  RCLCPP_WARN(
238  logger_, "The value of parameter '%s' is incorrectly set to %f, "
239  "it should be >=0. Ignoring parameter update.",
240  param_name.c_str(), parameter.as_double());
241  result.successful = false;
242  }
243  } else if (param_type == ParameterType::PARAMETER_BOOL) {
244  if (param_name == plugin_name_ + ".allow_reversing") {
245  if (params_.use_rotate_to_heading && parameter.as_bool()) {
246  RCLCPP_WARN(
247  logger_, "Both use_rotate_to_heading and allow_reversing "
248  "parameter cannot be set to true. Rejecting parameter update.");
249  result.successful = false;
250  }
251  }
252  }
253  }
254  return result;
255 }
256 void
257 ParameterHandler::updateParametersCallback(
258  std::vector<rclcpp::Parameter> parameters)
259 {
260  std::lock_guard<std::mutex> lock_reinit(mutex_);
261 
262  for (const auto & parameter : parameters) {
263  const auto & param_type = parameter.get_type();
264  const auto & param_name = parameter.get_name();
265 
266  if (param_type == ParameterType::PARAMETER_DOUBLE) {
267  if (param_name == plugin_name_ + ".inflation_cost_scaling_factor") {
268  params_.inflation_cost_scaling_factor = parameter.as_double();
269  } else if (param_name == plugin_name_ + ".desired_linear_vel") {
270  params_.desired_linear_vel = parameter.as_double();
271  params_.base_desired_linear_vel = parameter.as_double();
272  } else if (param_name == plugin_name_ + ".lookahead_dist") {
273  params_.lookahead_dist = parameter.as_double();
274  } else if (param_name == plugin_name_ + ".max_lookahead_dist") {
275  params_.max_lookahead_dist = parameter.as_double();
276  } else if (param_name == plugin_name_ + ".min_lookahead_dist") {
277  params_.min_lookahead_dist = parameter.as_double();
278  } else if (param_name == plugin_name_ + ".lookahead_time") {
279  params_.lookahead_time = parameter.as_double();
280  } else if (param_name == plugin_name_ + ".rotate_to_heading_angular_vel") {
281  params_.rotate_to_heading_angular_vel = parameter.as_double();
282  } else if (param_name == plugin_name_ + ".min_approach_linear_velocity") {
283  params_.min_approach_linear_velocity = parameter.as_double();
284  } else if (param_name == plugin_name_ + ".curvature_lookahead_dist") {
285  params_.curvature_lookahead_dist = parameter.as_double();
286  } else if (param_name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") {
287  params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double();
288  } else if (param_name == plugin_name_ + ".cost_scaling_dist") {
289  params_.cost_scaling_dist = parameter.as_double();
290  } else if (param_name == plugin_name_ + ".cost_scaling_gain") {
291  params_.cost_scaling_gain = parameter.as_double();
292  } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_radius") {
293  params_.regulated_linear_scaling_min_radius = parameter.as_double();
294  } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_speed") {
295  params_.regulated_linear_scaling_min_speed = parameter.as_double();
296  } else if (param_name == plugin_name_ + ".max_angular_accel") {
297  params_.max_angular_accel = parameter.as_double();
298  } else if (param_name == plugin_name_ + ".cancel_deceleration") {
299  params_.cancel_deceleration = parameter.as_double();
300  } else if (param_name == plugin_name_ + ".rotate_to_heading_min_angle") {
301  params_.rotate_to_heading_min_angle = parameter.as_double();
302  } else if (param_name == plugin_name_ + ".transform_tolerance") {
303  params_.transform_tolerance = parameter.as_double();
304  } else if (param_name == plugin_name_ + ".max_robot_pose_search_dist") {
305  params_.max_robot_pose_search_dist = parameter.as_double();
306  }
307  } else if (param_type == ParameterType::PARAMETER_BOOL) {
308  if (param_name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") {
309  params_.use_velocity_scaled_lookahead_dist = parameter.as_bool();
310  } else if (param_name == plugin_name_ + ".use_regulated_linear_velocity_scaling") {
311  params_.use_regulated_linear_velocity_scaling = parameter.as_bool();
312  } else if (param_name == plugin_name_ + ".use_fixed_curvature_lookahead") {
313  params_.use_fixed_curvature_lookahead = parameter.as_bool();
314  } else if (param_name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") {
315  params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool();
316  } else if (param_name == plugin_name_ + ".use_collision_detection") {
317  params_.use_collision_detection = parameter.as_bool();
318  } else if (param_name == plugin_name_ + ".stateful") {
319  params_.stateful = parameter.as_bool();
320  } else if (param_name == plugin_name_ + ".use_rotate_to_heading") {
321  params_.use_rotate_to_heading = parameter.as_bool();
322  } else if (param_name == plugin_name_ + ".use_cancel_deceleration") {
323  params_.use_cancel_deceleration = parameter.as_bool();
324  } else if (param_name == plugin_name_ + ".allow_reversing") {
325  params_.allow_reversing = parameter.as_bool();
326  } else if (param_name == plugin_name_ + ".interpolate_curvature_after_goal") {
327  params_.interpolate_curvature_after_goal = parameter.as_bool();
328  }
329  }
330  }
331 }
332 
333 } // 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:205
nav2_regulated_pure_pursuit_controller::ParameterHandler::ParameterHandler
ParameterHandler(rclcpp_lifecycle::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:257
Generated by doxygen 1.9.1