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  dyn_params_handler_ = node->add_on_set_parameters_callback(
119  std::bind(&ParameterHandler::dynamicParametersCallback, this, std::placeholders::_1));
120 }
121 
122 ParameterHandler::~ParameterHandler()
123 {
124  auto node = node_.lock();
125  if (dyn_params_handler_ && node) {
126  node->remove_on_set_parameters_callback(dyn_params_handler_.get());
127  }
128  dyn_params_handler_.reset();
129 }
130 
131 rcl_interfaces::msg::SetParametersResult
132 ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
133 {
134  rcl_interfaces::msg::SetParametersResult result;
135  std::lock_guard<std::mutex> lock_reinit(mutex_);
136 
137  for (auto parameter : parameters) {
138  const auto & param_type = parameter.get_type();
139  const auto & param_name = parameter.get_name();
140  if (param_name.find(plugin_name_ + ".") != 0) {
141  continue;
142  }
143 
144  if (param_type == ParameterType::PARAMETER_DOUBLE) {
145  if (param_name == plugin_name_ + ".transform_tolerance") {
146  params_.transform_tolerance = parameter.as_double();
147  } else if (param_name == plugin_name_ + ".min_lookahead") {
148  params_.min_lookahead = parameter.as_double();
149  } else if (param_name == plugin_name_ + ".max_lookahead") {
150  params_.max_lookahead = parameter.as_double();
151  } else if (param_name == plugin_name_ + ".k_phi") {
152  params_.k_phi = parameter.as_double();
153  } else if (param_name == plugin_name_ + ".k_delta") {
154  params_.k_delta = parameter.as_double();
155  } else if (param_name == plugin_name_ + ".beta") {
156  params_.beta = parameter.as_double();
157  } else if (param_name == plugin_name_ + ".lambda") {
158  params_.lambda = parameter.as_double();
159  } else if (param_name == plugin_name_ + ".v_linear_min") {
160  params_.v_linear_min = parameter.as_double();
161  } else if (param_name == plugin_name_ + ".v_linear_max") {
162  params_.v_linear_max = parameter.as_double();
163  params_.v_linear_max_initial = params_.v_linear_max;
164  } else if (param_name == plugin_name_ + ".v_angular_max") {
165  params_.v_angular_max = parameter.as_double();
166  params_.v_angular_max_initial = params_.v_angular_max;
167  } else if (param_name == plugin_name_ + ".v_angular_min_in_place") {
168  params_.v_angular_min_in_place = parameter.as_double();
169  } else if (param_name == plugin_name_ + ".slowdown_radius") {
170  params_.slowdown_radius = parameter.as_double();
171  } else if (param_name == plugin_name_ + ".initial_rotation_tolerance") {
172  params_.initial_rotation_tolerance = parameter.as_double();
173  } else if (param_name == plugin_name_ + ".rotation_scaling_factor") {
174  params_.rotation_scaling_factor = parameter.as_double();
175  } else if (param_name == plugin_name_ + ".in_place_collision_resolution") {
176  params_.in_place_collision_resolution = parameter.as_double();
177  }
178  } else if (param_type == ParameterType::PARAMETER_BOOL) {
179  if (param_name == plugin_name_ + ".initial_rotation") {
180  if (parameter.as_bool() && params_.allow_backward) {
181  RCLCPP_WARN(
182  logger_, "Initial rotation and allow backward parameters are both true, "
183  "rejecting parameter change.");
184  continue;
185  }
186  params_.initial_rotation = parameter.as_bool();
187  } else if (param_name == plugin_name_ + ".prefer_final_rotation") {
188  params_.prefer_final_rotation = parameter.as_bool();
189  } else if (param_name == plugin_name_ + ".allow_backward") {
190  if (params_.initial_rotation && parameter.as_bool()) {
191  RCLCPP_WARN(
192  logger_, "Initial rotation and allow backward parameters are both true, "
193  "rejecting parameter change.");
194  continue;
195  }
196  params_.allow_backward = parameter.as_bool();
197  } else if (param_name == plugin_name_ + ".use_collision_detection") {
198  params_.use_collision_detection = parameter.as_bool();
199  }
200  }
201  }
202 
203  result.successful = true;
204  return result;
205 }
206 
207 } // namespace nav2_graceful_controller
nav2_graceful_controller::ParameterHandler::~ParameterHandler
~ParameterHandler()
Destructor for nav2_graceful_controller::ParameterHandler.
Definition: parameter_handler.cpp:122
nav2_graceful_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: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
Generated by doxygen 1.9.1