Nav2 Navigation Stack - lyrical  lyrical
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 rcl_interfaces::msg::ParameterType;
28 
29 ParameterHandler::ParameterHandler(
30  const nav2::LifecycleNode::SharedPtr & node, std::string & plugin_name,
31  rclcpp::Logger & logger)
32 : nav2_util::ParameterHandler<Parameters>(node, logger)
33 {
34  plugin_name_ = plugin_name;
35 
36  params_.min_lookahead = node->declare_or_get_parameter(
37  plugin_name_ + ".min_lookahead", 0.25);
38  params_.max_lookahead = node->declare_or_get_parameter(
39  plugin_name_ + ".max_lookahead", 1.0);
40 
41  params_.k_phi = node->declare_or_get_parameter(
42  plugin_name_ + ".k_phi", 2.0);
43  params_.k_delta = node->declare_or_get_parameter(
44  plugin_name_ + ".k_delta", 1.0);
45  params_.beta = node->declare_or_get_parameter(
46  plugin_name_ + ".beta", 0.4);
47  params_.lambda = node->declare_or_get_parameter(
48  plugin_name_ + ".lambda", 2.0);
49  params_.v_linear_min = node->declare_or_get_parameter(
50  plugin_name_ + ".v_linear_min", 0.1);
51  params_.v_linear_max = node->declare_or_get_parameter(
52  plugin_name_ + ".v_linear_max", 0.5);
53  params_.v_angular_max = node->declare_or_get_parameter(
54  plugin_name_ + ".v_angular_max", 1.0);
55  params_.v_angular_min_in_place = node->declare_or_get_parameter(
56  plugin_name_ + ".v_angular_min_in_place", 0.25);
57  params_.slowdown_radius = node->declare_or_get_parameter(
58  plugin_name_ + ".slowdown_radius", 1.5);
59  params_.deceleration_max = node->declare_or_get_parameter(
60  plugin_name_ + ".deceleration_max", 2.5);
61  params_.initial_rotation = node->declare_or_get_parameter(
62  plugin_name_ + ".initial_rotation", true);
63  params_.initial_rotation_tolerance = node->declare_or_get_parameter(
64  plugin_name_ + ".initial_rotation_tolerance", 0.75);
65  params_.prefer_final_rotation = node->declare_or_get_parameter(
66  plugin_name_ + ".prefer_final_rotation", true);
67  params_.rotation_scaling_factor = node->declare_or_get_parameter(
68  plugin_name_ + ".rotation_scaling_factor", 0.5);
69  params_.allow_backward = node->declare_or_get_parameter(
70  plugin_name_ + ".allow_backward", false);
71  params_.in_place_collision_resolution = node->declare_or_get_parameter(
72  plugin_name_ + ".in_place_collision_resolution", 0.1);
73  params_.use_collision_detection = node->declare_or_get_parameter(
74  plugin_name_ + ".use_collision_detection", true);
75  params_.footprint_scaling_linear_vel = node->declare_or_get_parameter(
76  plugin_name_ + ".footprint_scaling_linear_vel", 0.5);
77  params_.footprint_scaling_factor = node->declare_or_get_parameter(
78  plugin_name_ + ".footprint_scaling_factor", 0.25);
79  params_.footprint_scaling_step = node->declare_or_get_parameter(
80  plugin_name_ + ".footprint_scaling_step", 0.1);
81  params_.obstacle_cost_margin = node->declare_or_get_parameter(
82  plugin_name_ + ".obstacle_cost_margin", 1);
83  params_.final_rotation_search_step = node->declare_or_get_parameter(
84  plugin_name_ + ".final_rotation_search_step", 0.1);
85  if (params_.initial_rotation && params_.allow_backward) {
86  RCLCPP_WARN(
87  logger_, "Initial rotation and allow backward parameters are both true, "
88  "setting allow backward to false.");
89  params_.allow_backward = false;
90  }
91 }
92 
93 rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback(
94  const std::vector<rclcpp::Parameter> & parameters)
95 {
96  rcl_interfaces::msg::SetParametersResult result;
97  result.successful = true;
98  for (const auto & parameter : parameters) {
99  const auto & param_type = parameter.get_type();
100  const auto & param_name = parameter.get_name();
101  if (param_name.find(plugin_name_ + ".") != 0) {
102  continue;
103  }
104  if (param_type == ParameterType::PARAMETER_DOUBLE) {
105  if (parameter.as_double() < 0.0) {
106  RCLCPP_WARN(
107  logger_, "The value of parameter '%s' is incorrectly set to %f, "
108  "it should be >=0. Ignoring parameter update.",
109  param_name.c_str(), parameter.as_double());
110  result.successful = false;
111  }
112  } else if (param_type == ParameterType::PARAMETER_BOOL) {
113  if (param_name == plugin_name_ + ".allow_backward") {
114  if (params_.initial_rotation && parameter.as_bool()) {
115  RCLCPP_WARN(
116  logger_, "Initial rotation and allow backward parameters are both true, "
117  "rejecting parameter change.");
118  result.successful = false;
119  }
120  } else if (param_name == plugin_name_ + ".initial_rotation") {
121  if (parameter.as_bool() && params_.allow_backward) {
122  RCLCPP_WARN(
123  logger_, "Initial rotation and allow backward parameters are both true, "
124  "rejecting parameter change.");
125  result.successful = false;
126  }
127  }
128  }
129  }
130  return result;
131 }
132 void
133 ParameterHandler::updateParametersCallback(
134  const std::vector<rclcpp::Parameter> & parameters)
135 {
136  std::lock_guard<std::mutex> lock_reinit(mutex_);
137 
138  for (const auto & parameter : parameters) {
139  const auto & param_type = parameter.get_type();
140  const auto & param_name = parameter.get_name();
141  if (param_name.find(plugin_name_ + ".") != 0) {
142  continue;
143  }
144  if (param_type == ParameterType::PARAMETER_DOUBLE) {
145  if (param_name == plugin_name_ + ".min_lookahead") {
146  params_.min_lookahead = parameter.as_double();
147  } else if (param_name == plugin_name_ + ".max_lookahead") {
148  params_.max_lookahead = parameter.as_double();
149  } else if (param_name == plugin_name_ + ".k_phi") {
150  params_.k_phi = parameter.as_double();
151  } else if (param_name == plugin_name_ + ".k_delta") {
152  params_.k_delta = parameter.as_double();
153  } else if (param_name == plugin_name_ + ".beta") {
154  params_.beta = parameter.as_double();
155  } else if (param_name == plugin_name_ + ".lambda") {
156  params_.lambda = parameter.as_double();
157  } else if (param_name == plugin_name_ + ".v_linear_min") {
158  params_.v_linear_min = parameter.as_double();
159  } else if (param_name == plugin_name_ + ".v_linear_max") {
160  params_.v_linear_max = parameter.as_double();
161  params_.v_linear_max_initial = params_.v_linear_max;
162  } else if (param_name == plugin_name_ + ".v_angular_max") {
163  params_.v_angular_max = parameter.as_double();
164  params_.v_angular_max_initial = params_.v_angular_max;
165  } else if (param_name == plugin_name_ + ".v_angular_min_in_place") {
166  params_.v_angular_min_in_place = parameter.as_double();
167  } else if (param_name == plugin_name_ + ".slowdown_radius") {
168  params_.slowdown_radius = parameter.as_double();
169  } else if (param_name == plugin_name_ + ".deceleration_max") {
170  params_.deceleration_max = 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  } else if (param_name == plugin_name_ + ".footprint_scaling_linear_vel") {
178  params_.footprint_scaling_linear_vel = parameter.as_double();
179  } else if (param_name == plugin_name_ + ".footprint_scaling_factor") {
180  params_.footprint_scaling_factor = parameter.as_double();
181  } else if (param_name == plugin_name_ + ".footprint_scaling_step") {
182  params_.footprint_scaling_step = parameter.as_double();
183  } else if (param_name == plugin_name_ + ".final_rotation_search_step") {
184  params_.final_rotation_search_step = parameter.as_double();
185  }
186  } else if (param_type == ParameterType::PARAMETER_BOOL) {
187  if (param_name == plugin_name_ + ".initial_rotation") {
188  params_.initial_rotation = parameter.as_bool();
189  } else if (param_name == plugin_name_ + ".prefer_final_rotation") {
190  params_.prefer_final_rotation = parameter.as_bool();
191  } else if (param_name == plugin_name_ + ".allow_backward") {
192  params_.allow_backward = parameter.as_bool();
193  } else if (param_name == plugin_name_ + ".use_collision_detection") {
194  params_.use_collision_detection = parameter.as_bool();
195  }
196  } else if (param_type == ParameterType::PARAMETER_INTEGER) {
197  if (param_name == plugin_name_ + ".obstacle_cost_margin") {
198  params_.obstacle_cost_margin = parameter.as_int();
199  }
200  }
201  }
202 }
203 
204 } // namespace nav2_graceful_controller
nav2_graceful_controller::ParameterHandler
Handles parameters and dynamic parameters for GracefulMotionController.
Definition: parameter_handler.hpp:66
nav2_graceful_controller::ParameterHandler::validateParameterUpdatesCallback
rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(const std::vector< rclcpp::Parameter > &parameters) override
Validate incoming parameter updates before applying them. This callback is triggered when one or more...
Definition: parameter_handler.cpp:93
nav2_graceful_controller::ParameterHandler::ParameterHandler
ParameterHandler(const nav2::LifecycleNode::SharedPtr &node, std::string &plugin_name, rclcpp::Logger &logger)
Constructor for nav2_graceful_controller::ParameterHandler.
Definition: parameter_handler.cpp:29
nav2_graceful_controller::ParameterHandler::updateParametersCallback
void updateParametersCallback(const std::vector< rclcpp::Parameter > &parameters) override
Apply parameter updates after validation This callback is executed when parameters have been successf...
Definition: parameter_handler.cpp:133
nav2_graceful_controller::Parameters
Definition: parameter_handler.hpp:32
Generated by doxygen 1.9.1