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 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  params_.v_linear_max_initial = params_.v_linear_max;
86  params_.v_angular_max_initial = params_.v_angular_max;
87 
88  if (params_.initial_rotation && params_.allow_backward) {
89  RCLCPP_WARN(
90  logger_, "Initial rotation and allow backward parameters are both true, "
91  "setting allow backward to false.");
92  params_.allow_backward = false;
93  }
94 }
95 
96 rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback(
97  const std::vector<rclcpp::Parameter> & parameters)
98 {
99  rcl_interfaces::msg::SetParametersResult result;
100  result.successful = true;
101  for (const auto & parameter : parameters) {
102  const auto & param_type = parameter.get_type();
103  const auto & param_name = parameter.get_name();
104  if (param_name.find(plugin_name_ + ".") != 0) {
105  continue;
106  }
107  if (param_type == ParameterType::PARAMETER_DOUBLE) {
108  if (parameter.as_double() < 0.0) {
109  RCLCPP_WARN(
110  logger_, "The value of parameter '%s' is incorrectly set to %f, "
111  "it should be >=0. Ignoring parameter update.",
112  param_name.c_str(), parameter.as_double());
113  result.successful = false;
114  }
115  } else if (param_type == ParameterType::PARAMETER_BOOL) {
116  if (param_name == plugin_name_ + ".allow_backward") {
117  if (params_.initial_rotation && parameter.as_bool()) {
118  RCLCPP_WARN(
119  logger_, "Initial rotation and allow backward parameters are both true, "
120  "rejecting parameter change.");
121  result.successful = false;
122  }
123  } else if (param_name == plugin_name_ + ".initial_rotation") {
124  if (parameter.as_bool() && params_.allow_backward) {
125  RCLCPP_WARN(
126  logger_, "Initial rotation and allow backward parameters are both true, "
127  "rejecting parameter change.");
128  result.successful = false;
129  }
130  }
131  }
132  }
133  return result;
134 }
135 void
136 ParameterHandler::updateParametersCallback(
137  const std::vector<rclcpp::Parameter> & parameters)
138 {
139  std::lock_guard<std::mutex> lock_reinit(mutex_);
140 
141  for (const auto & parameter : parameters) {
142  const auto & param_type = parameter.get_type();
143  const auto & param_name = parameter.get_name();
144  if (param_name.find(plugin_name_ + ".") != 0) {
145  continue;
146  }
147  if (param_type == ParameterType::PARAMETER_DOUBLE) {
148  if (param_name == plugin_name_ + ".min_lookahead") {
149  params_.min_lookahead = parameter.as_double();
150  } else if (param_name == plugin_name_ + ".max_lookahead") {
151  params_.max_lookahead = parameter.as_double();
152  } else if (param_name == plugin_name_ + ".k_phi") {
153  params_.k_phi = parameter.as_double();
154  } else if (param_name == plugin_name_ + ".k_delta") {
155  params_.k_delta = parameter.as_double();
156  } else if (param_name == plugin_name_ + ".beta") {
157  params_.beta = parameter.as_double();
158  } else if (param_name == plugin_name_ + ".lambda") {
159  params_.lambda = parameter.as_double();
160  } else if (param_name == plugin_name_ + ".v_linear_min") {
161  params_.v_linear_min = parameter.as_double();
162  } else if (param_name == plugin_name_ + ".v_linear_max") {
163  params_.v_linear_max = parameter.as_double();
164  params_.v_linear_max_initial = params_.v_linear_max;
165  } else if (param_name == plugin_name_ + ".v_angular_max") {
166  params_.v_angular_max = parameter.as_double();
167  params_.v_angular_max_initial = params_.v_angular_max;
168  } else if (param_name == plugin_name_ + ".v_angular_min_in_place") {
169  params_.v_angular_min_in_place = parameter.as_double();
170  } else if (param_name == plugin_name_ + ".slowdown_radius") {
171  params_.slowdown_radius = parameter.as_double();
172  } else if (param_name == plugin_name_ + ".deceleration_max") {
173  params_.deceleration_max = parameter.as_double();
174  } else if (param_name == plugin_name_ + ".initial_rotation_tolerance") {
175  params_.initial_rotation_tolerance = parameter.as_double();
176  } else if (param_name == plugin_name_ + ".rotation_scaling_factor") {
177  params_.rotation_scaling_factor = parameter.as_double();
178  } else if (param_name == plugin_name_ + ".in_place_collision_resolution") {
179  params_.in_place_collision_resolution = parameter.as_double();
180  } else if (param_name == plugin_name_ + ".footprint_scaling_linear_vel") {
181  params_.footprint_scaling_linear_vel = parameter.as_double();
182  } else if (param_name == plugin_name_ + ".footprint_scaling_factor") {
183  params_.footprint_scaling_factor = parameter.as_double();
184  } else if (param_name == plugin_name_ + ".footprint_scaling_step") {
185  params_.footprint_scaling_step = parameter.as_double();
186  } else if (param_name == plugin_name_ + ".final_rotation_search_step") {
187  params_.final_rotation_search_step = parameter.as_double();
188  }
189  } else if (param_type == ParameterType::PARAMETER_BOOL) {
190  if (param_name == plugin_name_ + ".initial_rotation") {
191  params_.initial_rotation = parameter.as_bool();
192  } else if (param_name == plugin_name_ + ".prefer_final_rotation") {
193  params_.prefer_final_rotation = parameter.as_bool();
194  } else if (param_name == plugin_name_ + ".allow_backward") {
195  params_.allow_backward = parameter.as_bool();
196  } else if (param_name == plugin_name_ + ".use_collision_detection") {
197  params_.use_collision_detection = parameter.as_bool();
198  }
199  } else if (param_type == ParameterType::PARAMETER_INTEGER) {
200  if (param_name == plugin_name_ + ".obstacle_cost_margin") {
201  params_.obstacle_cost_margin = parameter.as_int();
202  }
203  }
204  }
205 }
206 
207 } // 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:96
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:136
nav2_graceful_controller::Parameters
Definition: parameter_handler.hpp:32
Generated by doxygen 1.9.1