Nav2 Navigation Stack - humble  humble
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  plugin_name_ = plugin_name;
35  logger_ = logger;
36 
37  declare_parameter_if_not_declared(
38  node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1));
39  declare_parameter_if_not_declared(
40  node, plugin_name_ + ".motion_target_dist", rclcpp::ParameterValue(0.6));
41  declare_parameter_if_not_declared(
42  node, plugin_name_ + ".max_robot_pose_search_dist",
43  rclcpp::ParameterValue(costmap_size_x / 2.0));
44  declare_parameter_if_not_declared(node, plugin_name_ + ".k_phi", rclcpp::ParameterValue(3.0));
45  declare_parameter_if_not_declared(node, plugin_name_ + ".k_delta", rclcpp::ParameterValue(2.0));
46  declare_parameter_if_not_declared(node, plugin_name_ + ".beta", rclcpp::ParameterValue(0.2));
47  declare_parameter_if_not_declared(node, plugin_name_ + ".lambda", rclcpp::ParameterValue(2.0));
48  declare_parameter_if_not_declared(
49  node, plugin_name_ + ".v_linear_min", rclcpp::ParameterValue(0.1));
50  declare_parameter_if_not_declared(
51  node, plugin_name_ + ".v_linear_max", rclcpp::ParameterValue(0.5));
52  declare_parameter_if_not_declared(
53  node, plugin_name_ + ".v_angular_max", rclcpp::ParameterValue(1.0));
54  declare_parameter_if_not_declared(
55  node, plugin_name_ + ".slowdown_radius", rclcpp::ParameterValue(1.5));
56  declare_parameter_if_not_declared(
57  node, plugin_name_ + ".initial_rotation", rclcpp::ParameterValue(true));
58  declare_parameter_if_not_declared(
59  node, plugin_name_ + ".initial_rotation_min_angle", rclcpp::ParameterValue(0.75));
60  declare_parameter_if_not_declared(
61  node, plugin_name_ + ".final_rotation", rclcpp::ParameterValue(true));
62  declare_parameter_if_not_declared(
63  node, plugin_name_ + ".rotation_scaling_factor", rclcpp::ParameterValue(0.5));
64  declare_parameter_if_not_declared(
65  node, plugin_name_ + ".allow_backward", rclcpp::ParameterValue(false));
66 
67  node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance);
68  node->get_parameter(plugin_name_ + ".motion_target_dist", params_.motion_target_dist);
69  node->get_parameter(
70  plugin_name_ + ".max_robot_pose_search_dist", params_.max_robot_pose_search_dist);
71  if (params_.max_robot_pose_search_dist < 0.0) {
72  RCLCPP_WARN(
73  logger_, "Max robot search distance is negative, setting to max to search"
74  " every point on path for the closest value.");
75  params_.max_robot_pose_search_dist = std::numeric_limits<double>::max();
76  }
77 
78  node->get_parameter(plugin_name_ + ".k_phi", params_.k_phi);
79  node->get_parameter(plugin_name_ + ".k_delta", params_.k_delta);
80  node->get_parameter(plugin_name_ + ".beta", params_.beta);
81  node->get_parameter(plugin_name_ + ".lambda", params_.lambda);
82  node->get_parameter(plugin_name_ + ".v_linear_min", params_.v_linear_min);
83  node->get_parameter(plugin_name_ + ".v_linear_max", params_.v_linear_max);
84  params_.v_linear_max_initial = params_.v_linear_max;
85  node->get_parameter(plugin_name_ + ".v_angular_max", params_.v_angular_max);
86  params_.v_angular_max_initial = params_.v_angular_max;
87  node->get_parameter(plugin_name_ + ".slowdown_radius", params_.slowdown_radius);
88  node->get_parameter(plugin_name_ + ".initial_rotation", params_.initial_rotation);
89  node->get_parameter(
90  plugin_name_ + ".initial_rotation_min_angle", params_.initial_rotation_min_angle);
91  node->get_parameter(plugin_name_ + ".final_rotation", params_.final_rotation);
92  node->get_parameter(plugin_name_ + ".rotation_scaling_factor", params_.rotation_scaling_factor);
93  node->get_parameter(plugin_name_ + ".allow_backward", params_.allow_backward);
94 
95  if (params_.initial_rotation && params_.allow_backward) {
96  RCLCPP_WARN(
97  logger_, "Initial rotation and allow backward parameters are both true, "
98  "setting allow backward to false.");
99  params_.allow_backward = false;
100  }
101 
102  dyn_params_handler_ = node->add_on_set_parameters_callback(
103  std::bind(&ParameterHandler::dynamicParametersCallback, this, std::placeholders::_1));
104 }
105 
106 rcl_interfaces::msg::SetParametersResult
107 ParameterHandler::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
108 {
109  rcl_interfaces::msg::SetParametersResult result;
110  std::lock_guard<std::mutex> lock_reinit(mutex_);
111 
112  for (auto parameter : parameters) {
113  const auto & type = parameter.get_type();
114  const auto & name = parameter.get_name();
115 
116  if (type == ParameterType::PARAMETER_DOUBLE) {
117  if (name == plugin_name_ + ".transform_tolerance") {
118  params_.transform_tolerance = parameter.as_double();
119  } else if (name == plugin_name_ + ".motion_target_dist") {
120  params_.motion_target_dist = parameter.as_double();
121  } else if (name == plugin_name_ + ".k_phi") {
122  params_.k_phi = parameter.as_double();
123  } else if (name == plugin_name_ + ".k_delta") {
124  params_.k_delta = parameter.as_double();
125  } else if (name == plugin_name_ + ".beta") {
126  params_.beta = parameter.as_double();
127  } else if (name == plugin_name_ + ".lambda") {
128  params_.lambda = parameter.as_double();
129  } else if (name == plugin_name_ + ".v_linear_min") {
130  params_.v_linear_min = parameter.as_double();
131  } else if (name == plugin_name_ + ".v_linear_max") {
132  params_.v_linear_max = parameter.as_double();
133  params_.v_linear_max_initial = params_.v_linear_max;
134  } else if (name == plugin_name_ + ".v_angular_max") {
135  params_.v_angular_max = parameter.as_double();
136  params_.v_angular_max_initial = params_.v_angular_max;
137  } else if (name == plugin_name_ + ".slowdown_radius") {
138  params_.slowdown_radius = parameter.as_double();
139  } else if (name == plugin_name_ + ".initial_rotation_min_angle") {
140  params_.initial_rotation_min_angle = parameter.as_double();
141  } else if (name == plugin_name_ + ".rotation_scaling_factor") {
142  params_.rotation_scaling_factor = parameter.as_double();
143  }
144  } else if (type == ParameterType::PARAMETER_BOOL) {
145  if (name == plugin_name_ + ".initial_rotation") {
146  if (parameter.as_bool() && params_.allow_backward) {
147  RCLCPP_WARN(
148  logger_, "Initial rotation and allow backward parameters are both true, "
149  "rejecting parameter change.");
150  continue;
151  }
152  params_.initial_rotation = parameter.as_bool();
153  } else if (name == plugin_name_ + ".final_rotation") {
154  params_.final_rotation = parameter.as_bool();
155  } else if (name == plugin_name_ + ".allow_backward") {
156  if (params_.initial_rotation && parameter.as_bool()) {
157  RCLCPP_WARN(
158  logger_, "Initial rotation and allow backward parameters are both true, "
159  "rejecting parameter change.");
160  continue;
161  }
162  params_.allow_backward = parameter.as_bool();
163  }
164  }
165  }
166 
167  result.successful = true;
168  return result;
169 }
170 
171 } // namespace nav2_graceful_controller
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:107
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