Nav2 Navigation Stack - jazzy  jazzy
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  dyn_params_handler_ = node->add_on_set_parameters_callback(
196  std::bind(
197  &ParameterHandler::dynamicParametersCallback,
198  this, std::placeholders::_1));
199 }
200 
201 ParameterHandler::~ParameterHandler()
202 {
203  auto node = node_.lock();
204  if (dyn_params_handler_ && node) {
205  node->remove_on_set_parameters_callback(dyn_params_handler_.get());
206  }
207  dyn_params_handler_.reset();
208 }
209 
210 rcl_interfaces::msg::SetParametersResult
211 ParameterHandler::dynamicParametersCallback(
212  std::vector<rclcpp::Parameter> parameters)
213 {
214  rcl_interfaces::msg::SetParametersResult result;
215  std::lock_guard<std::mutex> lock_reinit(mutex_);
216 
217  for (auto parameter : parameters) {
218  const auto & type = parameter.get_type();
219  const auto & name = parameter.get_name();
220 
221  if (type == ParameterType::PARAMETER_DOUBLE) {
222  if (name == plugin_name_ + ".inflation_cost_scaling_factor") {
223  if (parameter.as_double() <= 0.0) {
224  RCLCPP_WARN(
225  logger_, "The value inflation_cost_scaling_factor is incorrectly set, "
226  "it should be >0. Ignoring parameter update.");
227  continue;
228  }
229  params_.inflation_cost_scaling_factor = parameter.as_double();
230  } else if (name == plugin_name_ + ".desired_linear_vel") {
231  params_.desired_linear_vel = parameter.as_double();
232  params_.base_desired_linear_vel = parameter.as_double();
233  } else if (name == plugin_name_ + ".lookahead_dist") {
234  params_.lookahead_dist = parameter.as_double();
235  } else if (name == plugin_name_ + ".max_lookahead_dist") {
236  params_.max_lookahead_dist = parameter.as_double();
237  } else if (name == plugin_name_ + ".min_lookahead_dist") {
238  params_.min_lookahead_dist = parameter.as_double();
239  } else if (name == plugin_name_ + ".lookahead_time") {
240  params_.lookahead_time = parameter.as_double();
241  } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") {
242  params_.rotate_to_heading_angular_vel = parameter.as_double();
243  } else if (name == plugin_name_ + ".min_approach_linear_velocity") {
244  params_.min_approach_linear_velocity = parameter.as_double();
245  } else if (name == plugin_name_ + ".curvature_lookahead_dist") {
246  params_.curvature_lookahead_dist = parameter.as_double();
247  } else if (name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") {
248  params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double();
249  } else if (name == plugin_name_ + ".cost_scaling_dist") {
250  params_.cost_scaling_dist = parameter.as_double();
251  } else if (name == plugin_name_ + ".cost_scaling_gain") {
252  params_.cost_scaling_gain = parameter.as_double();
253  } else if (name == plugin_name_ + ".regulated_linear_scaling_min_radius") {
254  params_.regulated_linear_scaling_min_radius = parameter.as_double();
255  } else if (name == plugin_name_ + ".regulated_linear_scaling_min_speed") {
256  params_.regulated_linear_scaling_min_speed = parameter.as_double();
257  } else if (name == plugin_name_ + ".max_angular_accel") {
258  params_.max_angular_accel = parameter.as_double();
259  } else if (name == plugin_name_ + ".cancel_deceleration") {
260  params_.cancel_deceleration = parameter.as_double();
261  } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") {
262  params_.rotate_to_heading_min_angle = parameter.as_double();
263  } else if (name == plugin_name_ + ".transform_tolerance") {
264  params_.transform_tolerance = parameter.as_double();
265  } else if (name == plugin_name_ + ".max_robot_pose_search_dist") {
266  params_.max_robot_pose_search_dist = parameter.as_double();
267  }
268  } else if (type == ParameterType::PARAMETER_BOOL) {
269  if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") {
270  params_.use_velocity_scaled_lookahead_dist = parameter.as_bool();
271  } else if (name == plugin_name_ + ".use_regulated_linear_velocity_scaling") {
272  params_.use_regulated_linear_velocity_scaling = parameter.as_bool();
273  } else if (name == plugin_name_ + ".use_fixed_curvature_lookahead") {
274  params_.use_fixed_curvature_lookahead = parameter.as_bool();
275  } else if (name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") {
276  params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool();
277  } else if (name == plugin_name_ + ".use_collision_detection") {
278  params_.use_collision_detection = parameter.as_bool();
279  } else if (name == plugin_name_ + ".stateful") {
280  params_.stateful = parameter.as_bool();
281  } else if (name == plugin_name_ + ".use_rotate_to_heading") {
282  params_.use_rotate_to_heading = parameter.as_bool();
283  } else if (name == plugin_name_ + ".use_cancel_deceleration") {
284  params_.use_cancel_deceleration = parameter.as_bool();
285  } else if (name == plugin_name_ + ".allow_reversing") {
286  if (params_.use_rotate_to_heading && parameter.as_bool()) {
287  RCLCPP_WARN(
288  logger_, "Both use_rotate_to_heading and allow_reversing "
289  "parameter cannot be set to true. Rejecting parameter update.");
290  continue;
291  }
292  params_.allow_reversing = parameter.as_bool();
293  } else if (name == plugin_name_ + ".interpolate_curvature_after_goal") {
294  params_.interpolate_curvature_after_goal = parameter.as_bool();
295  }
296  }
297  }
298 
299  result.successful = true;
300  return result;
301 }
302 
303 } // 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:201
nav2_regulated_pure_pursuit_controller::ParameterHandler::dynamicParametersCallback
rcl_interfaces::msg::SetParametersResult dynamicParametersCallback(std::vector< rclcpp::Parameter > parameters)
Callback executed when a parameter change is detected.
Definition: parameter_handler.cpp:211
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
Generated by doxygen 1.9.1