Skip to content

Commit b85fd6a

Browse files
mini-1235mini-1235
authored
Add namespace for Rotation Shim primary controller for better dynamic parameters handling (#5654)
* Redesign rotation shim dynamic parameters patterns Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Use new api Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add namespace for primary controller Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add warning for migration guide Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Lint Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Throw if parameter does not exist Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Clang tidy Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
1 parent e9fff3c commit b85fd6a

File tree

6 files changed

+358
-142
lines changed

6 files changed

+358
-142
lines changed

‎nav2_rotation_shim_controller/CMakeLists.txt‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,8 @@ nav2_package()
2424
set(library_name nav2_rotation_shim_controller)
2525

2626
add_library(${library_name} SHARED
27-
src/nav2_rotation_shim_controller.cpp)
27+
src/nav2_rotation_shim_controller.cpp
28+
src/parameter_handler.cpp)
2829
target_include_directories(${library_name}
2930
PUBLIC
3031
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"

‎nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp‎

Lines changed: 4 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@
2929
#include "nav2_core/controller_exceptions.hpp"
3030
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
3131
#include "nav2_controller/plugins/position_goal_checker.hpp"
32+
#include "nav2_rotation_shim_controller/parameter_handler.hpp"
3233

3334
namespace nav2_rotation_shim_controller
3435
{
@@ -162,13 +163,6 @@ class RotationShimController : public nav2_core::Controller
162163
*/
163164
bool isGoalChanged(const nav_msgs::msg::Path & path);
164165

165-
/**
166-
* @brief Callback executed when a parameter change is detected
167-
* @param event ParameterEvent message
168-
*/
169-
rcl_interfaces::msg::SetParametersResult
170-
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
171-
172166
nav2::LifecycleNode::WeakPtr node_;
173167
std::shared_ptr<tf2_ros::Buffer> tf_;
174168
std::string plugin_name_;
@@ -182,18 +176,11 @@ class RotationShimController : public nav2_core::Controller
182176
nav2_core::Controller::Ptr primary_controller_;
183177
bool path_updated_;
184178
nav_msgs::msg::Path current_path_;
185-
double forward_sampling_distance_, angular_dist_threshold_, angular_disengage_threshold_;
186-
double rotate_to_heading_angular_vel_, max_angular_accel_;
187-
double control_duration_, simulate_ahead_time_;
188-
bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_;
189-
bool closed_loop_;
190-
bool use_path_orientations_;
179+
Parameters * params_;
180+
bool in_rotation_;
191181
double last_angular_vel_ = std::numeric_limits<double>::max();
192-
193-
// Dynamic parameters handler
194-
std::mutex mutex_;
195-
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
196182
std::unique_ptr<nav2_controller::PositionGoalChecker> position_goal_checker_;
183+
std::unique_ptr<nav2_rotation_shim_controller::ParameterHandler> param_handler_;
197184
};
198185

199186
} // namespace nav2_rotation_shim_controller

‎nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/parameter_handler.hpp‎

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
// Copyright (c) 2022 Samsung Research America
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+
#ifndef NAV2_ROTATION_SHIM_CONTROLLER__PARAMETER_HANDLER_HPP_
16+
#define NAV2_ROTATION_SHIM_CONTROLLER__PARAMETER_HANDLER_HPP_
17+
18+
#include <string>
19+
#include <vector>
20+
#include <memory>
21+
#include <algorithm>
22+
#include <mutex>
23+
24+
#include "rclcpp/rclcpp.hpp"
25+
#include "nav2_ros_common/lifecycle_node.hpp"
26+
#include "nav2_util/odometry_utils.hpp"
27+
#include "nav2_util/geometry_utils.hpp"
28+
#include "nav2_ros_common/node_utils.hpp"
29+
30+
namespace nav2_rotation_shim_controller
31+
{
32+
33+
struct Parameters
34+
{
35+
double forward_sampling_distance;
36+
double angular_dist_threshold;
37+
double angular_disengage_threshold;
38+
double rotate_to_heading_angular_vel;
39+
double max_angular_accel;
40+
double simulate_ahead_time;
41+
double control_duration;
42+
bool rotate_to_goal_heading;
43+
bool rotate_to_heading_once;
44+
bool closed_loop;
45+
bool use_path_orientations;
46+
std::string primary_controller;
47+
};
48+
49+
/**
50+
* @class nav2_rotation_shim_controller::ParameterHandler
51+
* @brief Handles parameters and dynamic parameters for Rotation Shim
52+
*/
53+
class ParameterHandler
54+
{
55+
public:
56+
/**
57+
* @brief Constructor for nav2_rotation_shim_controller::ParameterHandler
58+
*/
59+
ParameterHandler(
60+
const nav2::LifecycleNode::SharedPtr & node,
61+
std::string & plugin_name,
62+
rclcpp::Logger & logger);
63+
64+
/**
65+
* @brief Destrructor for nav2_rotation_shim_controller::ParameterHandler
66+
*/
67+
~ParameterHandler();
68+
69+
std::mutex & getMutex() {return mutex_;}
70+
71+
Parameters * getParams() {return &params_;}
72+
73+
/**
74+
* @brief Registers callbacks for dynamic parameter handling.
75+
*/
76+
void activate();
77+
78+
/**
79+
* @brief Resets callbacks for dynamic parameter handling.
80+
*/
81+
void deactivate();
82+
83+
protected:
84+
nav2::LifecycleNode::WeakPtr node_;
85+
86+
/**
87+
* @brief Apply parameter updates after validation
88+
* This callback is executed when parameters have been successfully updated.
89+
* It updates the internal configuration of the node with the new parameter values.
90+
* @param parameters List of parameters that have been updated.
91+
*/
92+
void
93+
updateParametersCallback(const std::vector<rclcpp::Parameter> & parameters);
94+
95+
/**
96+
* @brief Validate incoming parameter updates before applying them.
97+
* This callback is triggered when one or more parameters are about to be updated.
98+
* It checks the validity of parameter values and rejects updates that would lead
99+
* to invalid or inconsistent configurations
100+
* @param parameters List of parameters that are being updated.
101+
* @return rcl_interfaces::msg::SetParametersResult Result indicating whether the update is accepted.
102+
*/
103+
rcl_interfaces::msg::SetParametersResult
104+
validateParameterUpdatesCallback(const std::vector<rclcpp::Parameter> & parameters);
105+
106+
// Dynamic parameters handler
107+
std::mutex mutex_;
108+
rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_;
109+
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_;
110+
Parameters params_;
111+
std::string plugin_name_;
112+
rclcpp::Logger logger_ {rclcpp::get_logger("RotationShimController")};
113+
};
114+
115+
} // namespace nav2_rotation_shim_controller
116+
117+
#endif // NAV2_ROTATION_SHIM_CONTROLLER__PARAMETER_HANDLER_HPP_

0 commit comments

Comments
 (0)