Nav2 Navigation Stack - lyrical  lyrical
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 rcl_interfaces::msg::ParameterType;
28 
29 ParameterHandler::ParameterHandler(
30  const nav2::LifecycleNode::SharedPtr & node,
31  std::string & plugin_name, rclcpp::Logger & logger,
32  const double costmap_size_x)
33 : nav2_util::ParameterHandler<Parameters>(node, logger)
34 {
35  plugin_name_ = plugin_name;
36 
37  const std::string old_name = plugin_name_ + ".desired_linear_vel";
38  const std::string new_name = plugin_name_ + ".max_linear_vel";
39  try {
40  params_.max_linear_vel = node->declare_or_get_parameter<double>(old_name);
41  RCLCPP_WARN(
42  logger_,
43  "Parameter '%s' is deprecated. Use '%s' instead.",
44  old_name.c_str(), new_name.c_str());
45  } catch (const std::exception &) {
46  params_.max_linear_vel = node->declare_or_get_parameter(new_name, 0.5);
47  }
48  params_.base_max_linear_vel = params_.max_linear_vel;
49 
50  params_.min_linear_vel =
51  node->declare_or_get_parameter(plugin_name_ + ".min_linear_vel", -0.5);
52  params_.max_angular_vel =
53  node->declare_or_get_parameter(plugin_name_ + ".max_angular_vel", 2.5);
54  params_.min_angular_vel =
55  node->declare_or_get_parameter(plugin_name_ + ".min_angular_vel", -2.5);
56  params_.lookahead_dist =
57  node->declare_or_get_parameter(plugin_name_ + ".lookahead_dist", 0.6);
58  params_.min_lookahead_dist =
59  node->declare_or_get_parameter(plugin_name_ + ".min_lookahead_dist", 0.3);
60  params_.max_lookahead_dist =
61  node->declare_or_get_parameter(plugin_name_ + ".max_lookahead_dist", 0.9);
62  params_.lookahead_time =
63  node->declare_or_get_parameter(plugin_name_ + ".lookahead_time", 1.5);
64  params_.rotate_to_heading_angular_vel = node->declare_or_get_parameter(
65  plugin_name_ + ".rotate_to_heading_angular_vel", 1.8);
66  params_.use_velocity_scaled_lookahead_dist = node->declare_or_get_parameter(
67  plugin_name_ + ".use_velocity_scaled_lookahead_dist", false);
68  params_.min_approach_linear_velocity = node->declare_or_get_parameter(
69  plugin_name_ + ".min_approach_linear_velocity", 0.05);
70  params_.approach_velocity_scaling_dist = node->declare_or_get_parameter(
71  plugin_name_ + ".approach_velocity_scaling_dist", 0.6);
72  if (params_.approach_velocity_scaling_dist > costmap_size_x / 2.0) {
73  RCLCPP_WARN(
74  logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, "
75  "leading to permanent slowdown");
76  }
77  params_.max_allowed_time_to_collision_up_to_carrot =
78  node->declare_or_get_parameter(
79  plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", 1.0);
80  params_.min_distance_to_obstacle = node->declare_or_get_parameter(
81  plugin_name_ + ".min_distance_to_obstacle", -1.0);
82  params_.use_regulated_linear_velocity_scaling =
83  node->declare_or_get_parameter(
84  plugin_name_ + ".use_regulated_linear_velocity_scaling", true);
85  params_.use_cost_regulated_linear_velocity_scaling =
86  node->declare_or_get_parameter(
87  plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", true);
88  params_.cost_scaling_dist =
89  node->declare_or_get_parameter(plugin_name_ + ".cost_scaling_dist", 0.6);
90  params_.cost_scaling_gain =
91  node->declare_or_get_parameter(plugin_name_ + ".cost_scaling_gain", 1.0);
92  params_.inflation_cost_scaling_factor = node->declare_or_get_parameter(
93  plugin_name_ + ".inflation_cost_scaling_factor", 3.0);
94  params_.regulated_linear_scaling_min_radius = node->declare_or_get_parameter(
95  plugin_name_ + ".regulated_linear_scaling_min_radius", 0.90);
96  params_.regulated_linear_scaling_min_speed = node->declare_or_get_parameter(
97  plugin_name_ + ".regulated_linear_scaling_min_speed", 0.25);
98  params_.use_fixed_curvature_lookahead = node->declare_or_get_parameter(
99  plugin_name_ + ".use_fixed_curvature_lookahead", false);
100  params_.curvature_lookahead_dist = node->declare_or_get_parameter(
101  plugin_name_ + ".curvature_lookahead_dist", 0.6);
102  params_.use_rotate_to_heading = node->declare_or_get_parameter(
103  plugin_name_ + ".use_rotate_to_heading", true);
104  params_.rotate_to_heading_min_angle = node->declare_or_get_parameter(
105  plugin_name_ + ".rotate_to_heading_min_angle", 0.785);
106  params_.max_linear_accel =
107  node->declare_or_get_parameter(plugin_name_ + ".max_linear_accel", 2.5);
108  params_.max_linear_decel =
109  node->declare_or_get_parameter(plugin_name_ + ".max_linear_decel", -2.5);
110  params_.max_angular_accel =
111  node->declare_or_get_parameter(plugin_name_ + ".max_angular_accel", 3.2);
112  params_.max_angular_decel =
113  node->declare_or_get_parameter(plugin_name_ + ".max_angular_decel", -3.2);
114  params_.use_cancel_deceleration = node->declare_or_get_parameter(
115  plugin_name_ + ".use_cancel_deceleration", false);
116  params_.cancel_deceleration = node->declare_or_get_parameter(
117  plugin_name_ + ".cancel_deceleration", 3.2);
118  params_.allow_reversing =
119  node->declare_or_get_parameter(plugin_name_ + ".allow_reversing", false);
120 
121  params_.interpolate_curvature_after_goal = node->declare_or_get_parameter(
122  plugin_name_ + ".interpolate_curvature_after_goal", false);
123  if (!params_.use_fixed_curvature_lookahead && params_.interpolate_curvature_after_goal) {
124  RCLCPP_WARN(
125  logger_, "For interpolate_curvature_after_goal to be set to true, "
126  "use_fixed_curvature_lookahead should be true, it is currently set to false. Disabling.");
127  params_.interpolate_curvature_after_goal = false;
128  }
129  params_.use_collision_detection = node->declare_or_get_parameter(
130  plugin_name_ + ".use_collision_detection", true);
131  if (params_.use_collision_detection && !params_.use_velocity_scaled_lookahead_dist &&
132  params_.min_distance_to_obstacle > params_.lookahead_dist)
133  {
134  RCLCPP_WARN(
135  logger_, "min_distance_to_obstacle (%.02f) is greater than lookahead_dist (%.02f). "
136  "The collision check distance will be capped by lookahead_dist.",
137  params_.min_distance_to_obstacle, params_.lookahead_dist);
138  }
139  if (params_.use_collision_detection && params_.use_velocity_scaled_lookahead_dist &&
140  params_.min_distance_to_obstacle > params_.max_lookahead_dist)
141  {
142  RCLCPP_WARN(
143  logger_, "min_distance_to_obstacle (%.02f) is greater than max_lookahead_dist (%.02f). "
144  "The collision check distance will be capped by max_lookahead_dist.",
145  params_.min_distance_to_obstacle, params_.max_lookahead_dist);
146  }
147  params_.use_dynamic_window =
148  node->declare_or_get_parameter(plugin_name_ + ".use_dynamic_window", false);
149  params_.allow_obstacle_checking_beyond_goal =
150  node->declare_or_get_parameter(plugin_name_ + ".allow_obstacle_checking_beyond_goal", false);
151  if (params_.allow_obstacle_checking_beyond_goal && !params_.use_velocity_scaled_lookahead_dist) {
152  RCLCPP_WARN(
153  logger_, "Parameter 'allow_obstacle_checking_beyond_goal' requires "
154  "'use_velocity_scaled_lookahead_dist' to be enabled.");
155  }
156  if (params_.allow_obstacle_checking_beyond_goal && params_.min_distance_to_obstacle <= 0.0) {
157  RCLCPP_WARN(
158  logger_,
159  "Parameter 'allow_obstacle_checking_beyond_goal' requires "
160  "'min_distance_to_obstacle' to be greater than 0.0. ");
161  }
162  if (params_.inflation_cost_scaling_factor <= 0.0) {
163  RCLCPP_WARN(
164  logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
165  "it should be >0. Disabling cost regulated linear velocity scaling.");
166  params_.use_cost_regulated_linear_velocity_scaling = false;
167  }
168 }
169 
170 rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback(
171  const std::vector<rclcpp::Parameter> & parameters)
172 {
173  rcl_interfaces::msg::SetParametersResult result;
174  result.successful = true;
175  for (const auto & parameter : parameters) {
176  const auto & param_type = parameter.get_type();
177  const auto & param_name = parameter.get_name();
178  if (param_name.find(plugin_name_ + ".") != 0) {
179  continue;
180  }
181  if (param_type == ParameterType::PARAMETER_DOUBLE) {
182  const bool allow_negative =
183  param_name == plugin_name_ + ".min_linear_vel" ||
184  param_name == plugin_name_ + ".min_angular_vel" ||
185  param_name == plugin_name_ + ".max_linear_decel" ||
186  param_name == plugin_name_ + ".max_angular_decel";
187  if (param_name == plugin_name_ + ".inflation_cost_scaling_factor" &&
188  parameter.as_double() <= 0.0)
189  {
190  RCLCPP_WARN(
191  logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
192  "it should be >0. Ignoring parameter update.");
193  result.successful = false;
194  } else if (parameter.as_double() < 0.0 && !allow_negative) {
195  RCLCPP_WARN(
196  logger_, "The value of parameter '%s' is incorrectly set to %f, "
197  "it should be >=0. Ignoring parameter update.",
198  param_name.c_str(), parameter.as_double());
199  result.successful = false;
200  }
201  } else if (param_type == ParameterType::PARAMETER_BOOL) {
202  if (param_name == plugin_name_ + ".allow_reversing") {
203  if (params_.use_rotate_to_heading && parameter.as_bool()) {
204  RCLCPP_WARN(
205  logger_, "Both use_rotate_to_heading and allow_reversing "
206  "parameter cannot be set to true. Rejecting parameter update.");
207  result.successful = false;
208  }
209  }
210  }
211  }
212  return result;
213 }
214 
215 void
216 ParameterHandler::updateParametersCallback(
217  const std::vector<rclcpp::Parameter> & parameters)
218 {
219  std::lock_guard<std::mutex> lock_reinit(mutex_);
220 
221  for (const auto & parameter : parameters) {
222  const auto & param_type = parameter.get_type();
223  const auto & param_name = parameter.get_name();
224  if (param_name.find(plugin_name_ + ".") != 0) {
225  continue;
226  }
227  if (param_type == ParameterType::PARAMETER_DOUBLE) {
228  if (param_name == plugin_name_ + ".inflation_cost_scaling_factor") {
229  params_.inflation_cost_scaling_factor = parameter.as_double();
230  } else if (param_name == plugin_name_ + ".max_linear_vel") {
231  params_.max_linear_vel = parameter.as_double();
232  params_.base_max_linear_vel = parameter.as_double();
233  } else if (param_name == plugin_name_ + ".desired_linear_vel") {
234  params_.max_linear_vel = parameter.as_double();
235  params_.base_max_linear_vel = parameter.as_double();
236  } else if (param_name == plugin_name_ + ".max_angular_accel") {
237  params_.max_angular_accel = parameter.as_double();
238  } else if (param_name == plugin_name_ + ".min_linear_vel") {
239  params_.min_linear_vel = parameter.as_double();
240  } else if (param_name == plugin_name_ + ".max_angular_vel") {
241  params_.max_angular_vel = parameter.as_double();
242  } else if (param_name == plugin_name_ + ".min_angular_vel") {
243  params_.min_angular_vel = parameter.as_double();
244  } else if (param_name == plugin_name_ + ".max_linear_accel") {
245  params_.max_linear_accel = parameter.as_double();
246  } else if (param_name == plugin_name_ + ".max_linear_decel") {
247  params_.max_linear_decel = parameter.as_double();
248  } else if (param_name == plugin_name_ + ".max_angular_decel") {
249  params_.max_angular_decel = parameter.as_double();
250  } else if (param_name == plugin_name_ + ".lookahead_dist") {
251  params_.lookahead_dist = parameter.as_double();
252  } else if (param_name == plugin_name_ + ".max_lookahead_dist") {
253  params_.max_lookahead_dist = parameter.as_double();
254  } else if (param_name == plugin_name_ + ".min_lookahead_dist") {
255  params_.min_lookahead_dist = parameter.as_double();
256  } else if (param_name == plugin_name_ + ".lookahead_time") {
257  params_.lookahead_time = parameter.as_double();
258  } else if (param_name == plugin_name_ + ".rotate_to_heading_angular_vel") {
259  params_.rotate_to_heading_angular_vel = parameter.as_double();
260  } else if (param_name == plugin_name_ + ".min_approach_linear_velocity") {
261  params_.min_approach_linear_velocity = parameter.as_double();
262  } else if (param_name == plugin_name_ + ".curvature_lookahead_dist") {
263  params_.curvature_lookahead_dist = parameter.as_double();
264  } else if (param_name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") {
265  params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double();
266  } else if (param_name == plugin_name_ + ".min_distance_to_obstacle") {
267  params_.min_distance_to_obstacle = parameter.as_double();
268  } else if (param_name == plugin_name_ + ".cost_scaling_dist") {
269  params_.cost_scaling_dist = parameter.as_double();
270  } else if (param_name == plugin_name_ + ".cost_scaling_gain") {
271  params_.cost_scaling_gain = parameter.as_double();
272  } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_radius") {
273  params_.regulated_linear_scaling_min_radius = parameter.as_double();
274  } else if (param_name == plugin_name_ + ".regulated_linear_scaling_min_speed") {
275  params_.regulated_linear_scaling_min_speed = parameter.as_double();
276  } else if (param_name == plugin_name_ + ".cancel_deceleration") {
277  params_.cancel_deceleration = parameter.as_double();
278  } else if (param_name == plugin_name_ + ".rotate_to_heading_min_angle") {
279  params_.rotate_to_heading_min_angle = parameter.as_double();
280  } else if (param_name == plugin_name_ + ".approach_velocity_scaling_dist") {
281  params_.approach_velocity_scaling_dist = parameter.as_double();
282  }
283  } else if (param_type == ParameterType::PARAMETER_BOOL) {
284  if (param_name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") {
285  params_.use_velocity_scaled_lookahead_dist = parameter.as_bool();
286  } else if (param_name == plugin_name_ + ".use_regulated_linear_velocity_scaling") {
287  params_.use_regulated_linear_velocity_scaling = parameter.as_bool();
288  } else if (param_name == plugin_name_ + ".use_fixed_curvature_lookahead") {
289  params_.use_fixed_curvature_lookahead = parameter.as_bool();
290  } else if (param_name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") {
291  params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool();
292  } else if (param_name == plugin_name_ + ".use_collision_detection") {
293  params_.use_collision_detection = parameter.as_bool();
294  } else if (param_name == plugin_name_ + ".use_rotate_to_heading") {
295  params_.use_rotate_to_heading = parameter.as_bool();
296  } else if (param_name == plugin_name_ + ".use_cancel_deceleration") {
297  params_.use_cancel_deceleration = parameter.as_bool();
298  } else if (param_name == plugin_name_ + ".allow_reversing") {
299  params_.allow_reversing = parameter.as_bool();
300  } else if (param_name == plugin_name_ + ".interpolate_curvature_after_goal") {
301  params_.interpolate_curvature_after_goal = parameter.as_bool();
302  } else if (param_name == plugin_name_ + ".use_dynamic_window") {
303  params_.use_dynamic_window = parameter.as_bool();
304  } else if (param_name == plugin_name_ + ".allow_obstacle_checking_beyond_goal") {
305  params_.allow_obstacle_checking_beyond_goal = parameter.as_bool();
306  }
307  }
308  }
309 }
310 
311 } // namespace nav2_regulated_pure_pursuit_controller
nav2_regulated_pure_pursuit_controller::ParameterHandler
Handles parameters and dynamic parameters for RPP.
Definition: parameter_handler.hpp:73
nav2_regulated_pure_pursuit_controller::ParameterHandler::ParameterHandler
ParameterHandler(const 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:29
nav2_regulated_pure_pursuit_controller::ParameterHandler::updateParametersCallback
void updateParametersCallback(const std::vector< rclcpp::Parameter > &parameters) override
Apply parameter updates after validation This callback is executed when parameters have been successf...
Definition: parameter_handler.cpp:216
nav2_regulated_pure_pursuit_controller::ParameterHandler::validateParameterUpdatesCallback
rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(const std::vector< rclcpp::Parameter > &parameters) override
Validate incoming parameter updates before applying them. This callback is triggered when one or more...
Definition: parameter_handler.cpp:170
nav2_regulated_pure_pursuit_controller::Parameters
Definition: parameter_handler.hpp:33
Generated by doxygen 1.9.1