Nav2 Navigation Stack - rolling  main
ROS 2 Navigation Stack
parameter_handler.cpp
1 // Copyright (c) 2023 Alberto J. Tudela Roldán
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_graceful_controller/parameter_handler.hpp"
23 
24 namespace nav2_graceful_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, std::string & plugin_name,
32  rclcpp::Logger & logger, const double costmap_size_x)
33 {
34  node_ = node;
35  plugin_name_ = plugin_name;
36  logger_ = logger;
37 
38  declare_parameter_if_not_declared(
39  node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1));
40  declare_parameter_if_not_declared(
41  node, plugin_name_ + ".min_lookahead", rclcpp::ParameterValue(0.25));
42  declare_parameter_if_not_declared(
43  node, plugin_name_ + ".max_lookahead", rclcpp::ParameterValue(1.0));
44  declare_parameter_if_not_declared(
45  node, plugin_name_ + ".max_robot_pose_search_dist",
46  rclcpp::ParameterValue(costmap_size_x / 2.0));
47  declare_parameter_if_not_declared(node, plugin_name_ + ".k_phi", rclcpp::ParameterValue(2.0));
48  declare_parameter_if_not_declared(node, plugin_name_ + ".k_delta", rclcpp::ParameterValue(1.0));
49  declare_parameter_if_not_declared(node, plugin_name_ + ".beta", rclcpp::ParameterValue(0.4));
50  declare_parameter_if_not_declared(node, plugin_name_ + ".lambda", rclcpp::ParameterValue(2.0));
51  declare_parameter_if_not_declared(
52  node, plugin_name_ + ".v_linear_min", rclcpp::ParameterValue(0.1));
53  declare_parameter_if_not_declared(
54  node, plugin_name_ + ".v_linear_max", rclcpp::ParameterValue(0.5));
55  declare_parameter_if_not_declared(
56  node, plugin_name_ + ".v_angular_max", rclcpp::ParameterValue(1.0));
57  declare_parameter_if_not_declared(
58  node, plugin_name_ + ".v_angular_min_in_place", rclcpp::ParameterValue(0.25));
59  declare_parameter_if_not_declared(
60  node, plugin_name_ + ".slowdown_radius", rclcpp::ParameterValue(1.5));
61  declare_parameter_if_not_declared(
62  node, plugin_name_ + ".initial_rotation", rclcpp::ParameterValue(true));
63  declare_parameter_if_not_declared(
64  node, plugin_name_ + ".initial_rotation_tolerance", rclcpp::ParameterValue(0.75));
65  declare_parameter_if_not_declared(
66  node, plugin_name_ + ".prefer_final_rotation", rclcpp::ParameterValue(true));
67  declare_parameter_if_not_declared(
68  node, plugin_name_ + ".rotation_scaling_factor", rclcpp::ParameterValue(0.5));
69  declare_parameter_if_not_declared(
70  node, plugin_name_ + ".allow_backward", rclcpp::ParameterValue(false));
71  declare_parameter_if_not_declared(
72  node, plugin_name_ + ".in_place_collision_resolution", rclcpp::ParameterValue(0.1));
73  declare_parameter_if_not_declared(
74  node, plugin_name_ + ".use_collision_detection", rclcpp::ParameterValue(true));
75 
76  node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance);
77  node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead);
78  node->get_parameter(plugin_name_ + ".max_lookahead", params_.max_lookahead);
79  node->get_parameter(
80  plugin_name_ + ".max_robot_pose_search_dist", params_.max_robot_pose_search_dist);
81  if (params_.max_robot_pose_search_dist < 0.0) {
82  RCLCPP_WARN(
83  logger_, "Max robot search distance is negative, setting to max to search"
84  " every point on path for the closest value.");
85  params_.max_robot_pose_search_dist = std::numeric_limits<double>::max();
86  }
87 
88  node->get_parameter(plugin_name_ + ".k_phi", params_.k_phi);
89  node->get_parameter(plugin_name_ + ".k_delta", params_.k_delta);
90  node->get_parameter(plugin_name_ + ".beta", params_.beta);
91  node->get_parameter(plugin_name_ + ".lambda", params_.lambda);
92  node->get_parameter(plugin_name_ + ".v_linear_min", params_.v_linear_min);
93  node->get_parameter(plugin_name_ + ".v_linear_max", params_.v_linear_max);
94  params_.v_linear_max_initial = params_.v_linear_max;
95  node->get_parameter(plugin_name_ + ".v_angular_max", params_.v_angular_max);
96  params_.v_angular_max_initial = params_.v_angular_max;
97  node->get_parameter(
98  plugin_name_ + ".v_angular_min_in_place", params_.v_angular_min_in_place);
99  node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius);
100  node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation);
101  node->get_parameter(
102  plugin_name_ + ".initial_rotation_tolerance", params_.initial_rotation_tolerance);
103  node->get_parameter(plugin_name_ + ".prefer_final_rotation", params_.prefer_final_rotation);
104  node->get_parameter(plugin_name_ + ".rotation_scaling_factor", params_.rotation_scaling_factor);
105  node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward);
106  node->get_parameter(
107  plugin_name_ + ".in_place_collision_resolution", params_.in_place_collision_resolution);
108  node->get_parameter(
109  plugin_name_ + ".use_collision_detection", params_.use_collision_detection);
110 
111  if (params_.initial_rotation && params_.allow_backward) {
112  RCLCPP_WARN(
113  logger_, "Initial rotation and allow backward parameters are both true, "
114  "setting allow backward to false.");
115  params_.allow_backward = false;
116  }
117 }
118 
119 void ParameterHandler::activate()
120 {
121  auto node = node_.lock();
122  post_set_params_handler_ = node->add_post_set_parameters_callback(
123  std::bind(
124  &ParameterHandler::updateParametersCallback,
125  this, std::placeholders::_1));
126  on_set_params_handler_ = node->add_on_set_parameters_callback(
127  std::bind(
128  &ParameterHandler::validateParameterUpdatesCallback,
129  this, std::placeholders::_1));
130 }
131 
132 void ParameterHandler::deactivate()
133 {
134  auto node = node_.lock();
135  if (post_set_params_handler_ && node) {
136  node->remove_post_set_parameters_callback(post_set_params_handler_.get());
137  }
138  post_set_params_handler_.reset();
139  if (on_set_params_handler_ && node) {
140  node->remove_on_set_parameters_callback(on_set_params_handler_.get());
141  }
142  on_set_params_handler_.reset();
143 }
144 
145 ParameterHandler::~ParameterHandler()
146 {
147 }
148 
149 rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback(
150  std::vector<rclcpp::Parameter> parameters)
151 {
152  rcl_interfaces::msg::SetParametersResult result;
153  result.successful = true;
154  for (auto parameter : parameters) {
155  const auto & param_type = parameter.get_type();
156  const auto & param_name = parameter.get_name();
157  if (param_name.find(plugin_name_ + ".") != 0) {
158  continue;
159  }
160  if (param_type == ParameterType::PARAMETER_DOUBLE) {
161  if (parameter.as_double() < 0.0) {
162  RCLCPP_WARN(
163  logger_, "The value of parameter '%s' is incorrectly set to %f, "
164  "it should be >=0. Ignoring parameter update.",
165  param_name.c_str(), parameter.as_double());
166  result.successful = false;
167  }
168  } else if (param_type == ParameterType::PARAMETER_BOOL) {
169  if (param_name == plugin_name_ + ".allow_backward") {
170  if (params_.initial_rotation && parameter.as_bool()) {
171  RCLCPP_WARN(
172  logger_, "Initial rotation and allow backward parameters are both true, "
173  "rejecting parameter change.");
174  result.successful = false;
175  }
176  } else if(param_name == plugin_name_ + ".initial_rotation") {
177  if (parameter.as_bool() && params_.allow_backward) {
178  RCLCPP_WARN(
179  logger_, "Initial rotation and allow backward parameters are both true, "
180  "rejecting parameter change.");
181  result.successful = false;
182  }
183  }
184  }
185  }
186  return result;
187 }
188 void
189 ParameterHandler::updateParametersCallback(
190  std::vector<rclcpp::Parameter> parameters)
191 {
192  std::lock_guard<std::mutex> lock_reinit(mutex_);
193 
194  for (const auto & parameter : parameters) {
195  const auto & param_type = parameter.get_type();
196  const auto & param_name = parameter.get_name();
197  if (param_name.find(plugin_name_ + ".") != 0) {
198  continue;
199  }
200  if (param_type == ParameterType::PARAMETER_DOUBLE) {
201  if (param_name == plugin_name_ + ".transform_tolerance") {
202  params_.transform_tolerance = parameter.as_double();
203  } else if (param_name == plugin_name_ + ".min_lookahead") {
204  params_.min_lookahead = parameter.as_double();
205  } else if (param_name == plugin_name_ + ".max_lookahead") {
206  params_.max_lookahead = parameter.as_double();
207  } else if (param_name == plugin_name_ + ".k_phi") {
208  params_.k_phi = parameter.as_double();
209  } else if (param_name == plugin_name_ + ".k_delta") {
210  params_.k_delta = parameter.as_double();
211  } else if (param_name == plugin_name_ + ".beta") {
212  params_.beta = parameter.as_double();
213  } else if (param_name == plugin_name_ + ".lambda") {
214  params_.lambda = parameter.as_double();
215  } else if (param_name == plugin_name_ + ".v_linear_min") {
216  params_.v_linear_min = parameter.as_double();
217  } else if (param_name == plugin_name_ + ".v_linear_max") {
218  params_.v_linear_max = parameter.as_double();
219  params_.v_linear_max_initial = params_.v_linear_max;
220  } else if (param_name == plugin_name_ + ".v_angular_max") {
221  params_.v_angular_max = parameter.as_double();
222  params_.v_angular_max_initial = params_.v_angular_max;
223  } else if (param_name == plugin_name_ + ".v_angular_min_in_place") {
224  params_.v_angular_min_in_place = parameter.as_double();
225  } else if (param_name == plugin_name_ + ".slowdown_radius") {
226  params_.slowdown_radius = parameter.as_double();
227  } else if (param_name == plugin_name_ + ".initial_rotation_tolerance") {
228  params_.initial_rotation_tolerance = parameter.as_double();
229  } else if (param_name == plugin_name_ + ".rotation_scaling_factor") {
230  params_.rotation_scaling_factor = parameter.as_double();
231  } else if (param_name == plugin_name_ + ".in_place_collision_resolution") {
232  params_.in_place_collision_resolution = parameter.as_double();
233  }
234  } else if (param_type == ParameterType::PARAMETER_BOOL) {
235  if (param_name == plugin_name_ + ".initial_rotation") {
236  params_.initial_rotation = parameter.as_bool();
237  } else if (param_name == plugin_name_ + ".prefer_final_rotation") {
238  params_.prefer_final_rotation = parameter.as_bool();
239  } else if (param_name == plugin_name_ + ".allow_backward") {
240  params_.allow_backward = parameter.as_bool();
241  } else if (param_name == plugin_name_ + ".use_collision_detection") {
242  params_.use_collision_detection = parameter.as_bool();
243  }
244  }
245  }
246 }
247 
248 } // namespace nav2_graceful_controller
nav2_graceful_controller::ParameterHandler::activate
void activate()
Registers callbacks for dynamic parameter handling.
Definition: parameter_handler.cpp:119
nav2_graceful_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:189
nav2_graceful_controller::ParameterHandler::~ParameterHandler
~ParameterHandler()
Destructor for nav2_graceful_controller::ParameterHandler.
Definition: parameter_handler.cpp:145
nav2_graceful_controller::ParameterHandler::deactivate
void deactivate()
Resets callbacks for dynamic parameter handling.
Definition: parameter_handler.cpp:132
nav2_graceful_controller::ParameterHandler::ParameterHandler
ParameterHandler(nav2::LifecycleNode::SharedPtr node, std::string &plugin_name, rclcpp::Logger &logger, const double costmap_size_x)
Constructor for nav2_graceful_controller::ParameterHandler.
Definition: parameter_handler.cpp:30
nav2_graceful_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:149
Generated by doxygen 1.9.1