Nav2 Navigation Stack - jazzy  jazzy
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_util::declare_parameter_if_not_declared;
28 using rcl_interfaces::msg::ParameterType;
29 
30 ParameterHandler::ParameterHandler(
31  rclcpp_lifecycle::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 
74  node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance);
75  node->get_parameter(plugin_name_ + ".min_lookahead", params_.min_lookahead);
76  node->get_parameter(plugin_name_ + ".max_lookahead", params_.max_lookahead);
77  node->get_parameter(
78  plugin_name_ + ".max_robot_pose_search_dist", params_.max_robot_pose_search_dist);
79  if (params_.max_robot_pose_search_dist < 0.0) {
80  RCLCPP_WARN(
81  logger_, "Max robot search distance is negative, setting to max to search"
82  " every point on path for the closest value.");
83  params_.max_robot_pose_search_dist = std::numeric_limits<double>::max();
84  }
85 
86  node->get_parameter(plugin_name_ + ".k_phi", params_.k_phi);
87  node->get_parameter(plugin_name_ + ".k_delta", params_.k_delta);
88  node->get_parameter(plugin_name_ + ".beta", params_.beta);
89  node->get_parameter(plugin_name_ + ".lambda", params_.lambda);
90  node->get_parameter(plugin_name_ + ".v_linear_min", params_.v_linear_min);
91  node->get_parameter(plugin_name_ + ".v_linear_max", params_.v_linear_max);
92  params_.v_linear_max_initial = params_.v_linear_max;
93  node->get_parameter(plugin_name_ + ".v_angular_max", params_.v_angular_max);
94  params_.v_angular_max_initial = params_.v_angular_max;
95  node->get_parameter(
96  plugin_name_ + ".v_angular_min_in_place", params_.v_angular_min_in_place);
97  node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius);
98  node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation);
99  node->get_parameter(
100  plugin_name_ + ".initial_rotation_tolerance", params_.initial_rotation_tolerance);
101  node->get_parameter(plugin_name_ + ".prefer_final_rotation", params_.prefer_final_rotation);
102  node->get_parameter(plugin_name_ + ".rotation_scaling_factor", params_.rotation_scaling_factor);
103  node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward);
104  node->get_parameter(
105  plugin_name_ + ".in_place_collision_resolution", params_.in_place_collision_resolution);
106 
107  if (params_.initial_rotation && params_.allow_backward) {
108  RCLCPP_WARN(
109  logger_, "Initial rotation and allow backward parameters are both true, "
110  "setting allow backward to false.");
111  params_.allow_backward = false;
112  }
113 
114  dyn_params_handler_ = node->add_on_set_parameters_callback(
115  std::bind(&ParameterHandler::dynamicParametersCallback, this, std::placeholders::_1));
116 }
117 
118 ParameterHandler::~ParameterHandler()
119 {
120  auto node = node_.lock();
121  if (dyn_params_handler_ && node) {
122  node->remove_on_set_parameters_callback(dyn_params_handler_.get());
123  }
124  dyn_params_handler_.reset();
125 }
126 
127 rcl_interfaces::msg::SetParametersResult
128 ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
129 {
130  rcl_interfaces::msg::SetParametersResult result;
131  std::lock_guard<std::mutex> lock_reinit(mutex_);
132 
133  for (auto parameter : parameters) {
134  const auto & type = parameter.get_type();
135  const auto & name = parameter.get_name();
136 
137  if (type == ParameterType::PARAMETER_DOUBLE) {
138  if (name == plugin_name_ + ".transform_tolerance") {
139  params_.transform_tolerance = parameter.as_double();
140  } else if (name == plugin_name_ + ".min_lookahead") {
141  params_.min_lookahead = parameter.as_double();
142  } else if (name == plugin_name_ + ".max_lookahead") {
143  params_.max_lookahead = parameter.as_double();
144  } else if (name == plugin_name_ + ".k_phi") {
145  params_.k_phi = parameter.as_double();
146  } else if (name == plugin_name_ + ".k_delta") {
147  params_.k_delta = parameter.as_double();
148  } else if (name == plugin_name_ + ".beta") {
149  params_.beta = parameter.as_double();
150  } else if (name == plugin_name_ + ".lambda") {
151  params_.lambda = parameter.as_double();
152  } else if (name == plugin_name_ + ".v_linear_min") {
153  params_.v_linear_min = parameter.as_double();
154  } else if (name == plugin_name_ + ".v_linear_max") {
155  params_.v_linear_max = parameter.as_double();
156  params_.v_linear_max_initial = params_.v_linear_max;
157  } else if (name == plugin_name_ + ".v_angular_max") {
158  params_.v_angular_max = parameter.as_double();
159  params_.v_angular_max_initial = params_.v_angular_max;
160  } else if (name == plugin_name_ + ".v_angular_min_in_place") {
161  params_.v_angular_min_in_place = parameter.as_double();
162  } else if (name == plugin_name_ + ".slowdown_radius") {
163  params_.slowdown_radius = parameter.as_double();
164  } else if (name == plugin_name_ + ".initial_rotation_tolerance") {
165  params_.initial_rotation_tolerance = parameter.as_double();
166  } else if (name == plugin_name_ + ".rotation_scaling_factor") {
167  params_.rotation_scaling_factor = parameter.as_double();
168  } else if (name == plugin_name_ + ".in_place_collision_resolution") {
169  params_.in_place_collision_resolution = parameter.as_double();
170  }
171  } else if (type == ParameterType::PARAMETER_BOOL) {
172  if (name == plugin_name_ + ".initial_rotation") {
173  if (parameter.as_bool() && params_.allow_backward) {
174  RCLCPP_WARN(
175  logger_, "Initial rotation and allow backward parameters are both true, "
176  "rejecting parameter change.");
177  continue;
178  }
179  params_.initial_rotation = parameter.as_bool();
180  } else if (name == plugin_name_ + ".prefer_final_rotation") {
181  params_.prefer_final_rotation = parameter.as_bool();
182  } else if (name == plugin_name_ + ".allow_backward") {
183  if (params_.initial_rotation && parameter.as_bool()) {
184  RCLCPP_WARN(
185  logger_, "Initial rotation and allow backward parameters are both true, "
186  "rejecting parameter change.");
187  continue;
188  }
189  params_.allow_backward = parameter.as_bool();
190  }
191  }
192  }
193 
194  result.successful = true;
195  return result;
196 }
197 
198 } // namespace nav2_graceful_controller
nav2_graceful_controller::ParameterHandler::~ParameterHandler
~ParameterHandler()
Destructor for nav2_graceful_controller::ParameterHandler.
Definition: parameter_handler.cpp:118
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:128
nav2_graceful_controller::ParameterHandler::ParameterHandler
ParameterHandler(rclcpp_lifecycle::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