Skip to content

Commit 29ba4b1

Browse files
ajtudelaajtudela
authored
Added dynamic reconfigure and export controller in docking (#4429)
* Added dynamic reconfigure and export controller in docking Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Fix vector Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Update controller.cpp Signed-off-by: Alberto Tudela <ajtudela@gmail.com> --------- Signed-off-by: Alberto Tudela <ajtudela@gmail.com>
1 parent 23ddd86 commit 29ba4b1

File tree

4 files changed

+117
-15
lines changed

4 files changed

+117
-15
lines changed

‎nav2_docking/opennav_docking/CMakeLists.txt‎

Lines changed: 13 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -56,8 +56,15 @@ set(dependencies
5656
opennav_docking_core
5757
)
5858

59-
add_library(${library_name} SHARED
59+
add_library(controller SHARED
6060
src/controller.cpp
61+
)
62+
63+
ament_target_dependencies(controller
64+
${dependencies}
65+
)
66+
67+
add_library(${library_name} SHARED
6168
src/docking_server.cpp
6269
src/dock_database.cpp
6370
src/navigator.cpp
@@ -68,7 +75,9 @@ ament_target_dependencies(${library_name}
6875
)
6976

7077
target_link_libraries(${library_name}
71-
yaml-cpp::yaml-cpp)
78+
yaml-cpp::yaml-cpp
79+
controller
80+
)
7281

7382
add_library(pose_filter SHARED
7483
src/pose_filter.cpp
@@ -100,7 +109,7 @@ target_link_libraries(simple_charging_dock pose_filter)
100109

101110
pluginlib_export_plugin_description_file(opennav_docking_core plugins.xml)
102111

103-
install(TARGETS ${library_name} pose_filter simple_charging_dock
112+
install(TARGETS ${library_name} controller pose_filter simple_charging_dock
104113
ARCHIVE DESTINATION lib
105114
LIBRARY DESTINATION lib
106115
RUNTIME DESTINATION bin
@@ -127,6 +136,6 @@ if(BUILD_TESTING)
127136
endif()
128137

129138
ament_export_include_directories(include)
130-
ament_export_libraries(${library_name} pose_filter)
139+
ament_export_libraries(${library_name} controller pose_filter)
131140
ament_export_dependencies(${dependencies} yaml-cpp)
132141
ament_package()

‎nav2_docking/opennav_docking/include/opennav_docking/controller.hpp‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#define OPENNAV_DOCKING__CONTROLLER_HPP_
1717

1818
#include <memory>
19+
#include <vector>
1920

2021
#include "geometry_msgs/msg/pose.hpp"
2122
#include "geometry_msgs/msg/twist.hpp"
@@ -47,8 +48,22 @@ class Controller
4748
const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd,
4849
bool backward = false);
4950

51+
/**
52+
* @brief Callback executed when a parameter change is detected
53+
* @param event ParameterEvent message
54+
*/
55+
rcl_interfaces::msg::SetParametersResult
56+
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
57+
58+
// Dynamic parameters handler
59+
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
60+
std::mutex dynamic_params_lock_;
61+
5062
protected:
5163
std::unique_ptr<nav2_graceful_controller::SmoothControlLaw> control_law_;
64+
65+
double k_phi_, k_delta_, beta_, lambda_;
66+
double slowdown_radius_, v_linear_min_, v_linear_max_, v_angular_max_;
5267
};
5368

5469
} // namespace opennav_docking

‎nav2_docking/opennav_docking/src/controller.cpp‎

Lines changed: 55 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -40,25 +40,69 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
4040
nav2_util::declare_parameter_if_not_declared(
4141
node, "controller.slowdown_radius", rclcpp::ParameterValue(0.25));
4242

43-
double k_phi, k_delta, beta, lambda;
44-
double slowdown_radius, v_linear_min, v_linear_max, v_angular_max;
45-
node->get_parameter("controller.k_phi", k_phi);
46-
node->get_parameter("controller.k_delta", k_delta);
47-
node->get_parameter("controller.beta", beta);
48-
node->get_parameter("controller.lambda", lambda);
49-
node->get_parameter("controller.v_linear_min", v_linear_min);
50-
node->get_parameter("controller.v_linear_max", v_linear_max);
51-
node->get_parameter("controller.v_angular_max", v_angular_max);
52-
node->get_parameter("controller.slowdown_radius", slowdown_radius);
43+
node->get_parameter("controller.k_phi", k_phi_);
44+
node->get_parameter("controller.k_delta", k_delta_);
45+
node->get_parameter("controller.beta", beta_);
46+
node->get_parameter("controller.lambda", lambda_);
47+
node->get_parameter("controller.v_linear_min", v_linear_min_);
48+
node->get_parameter("controller.v_linear_max", v_linear_max_);
49+
node->get_parameter("controller.v_angular_max", v_angular_max_);
50+
node->get_parameter("controller.slowdown_radius", slowdown_radius_);
5351
control_law_ = std::make_unique<nav2_graceful_controller::SmoothControlLaw>(
54-
k_phi, k_delta, beta, lambda, slowdown_radius, v_linear_min, v_linear_max, v_angular_max);
52+
k_phi_, k_delta_, beta_, lambda_, slowdown_radius_, v_linear_min_, v_linear_max_,
53+
v_angular_max_);
54+
55+
// Add callback for dynamic parameters
56+
dyn_params_handler_ = node->add_on_set_parameters_callback(
57+
std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1));
5558
}
5659

5760
bool Controller::computeVelocityCommand(
5861
const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Twist & cmd, bool backward)
5962
{
63+
std::lock_guard<std::mutex> lock(dynamic_params_lock_);
6064
cmd = control_law_->calculateRegularVelocity(pose, backward);
6165
return true;
6266
}
6367

68+
rcl_interfaces::msg::SetParametersResult
69+
Controller::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
70+
{
71+
std::lock_guard<std::mutex> lock(dynamic_params_lock_);
72+
73+
rcl_interfaces::msg::SetParametersResult result;
74+
for (auto parameter : parameters) {
75+
const auto & type = parameter.get_type();
76+
const auto & name = parameter.get_name();
77+
78+
if (type == rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE) {
79+
if (name == "controller.k_phi") {
80+
k_phi_ = parameter.as_double();
81+
} else if (name == "controller.k_delta") {
82+
k_delta_ = parameter.as_double();
83+
} else if (name == "controller.beta") {
84+
beta_ = parameter.as_double();
85+
} else if (name == "controller.lambda") {
86+
lambda_ = parameter.as_double();
87+
} else if (name == "controller.v_linear_min") {
88+
v_linear_min_ = parameter.as_double();
89+
} else if (name == "controller.v_linear_max") {
90+
v_linear_max_ = parameter.as_double();
91+
} else if (name == "controller.v_angular_max") {
92+
v_angular_max_ = parameter.as_double();
93+
} else if (name == "controller.slowdown_radius") {
94+
slowdown_radius_ = parameter.as_double();
95+
}
96+
97+
// Update the smooth control law with the new params
98+
control_law_->setCurvatureConstants(k_phi_, k_delta_, beta_, lambda_);
99+
control_law_->setSlowdownRadius(slowdown_radius_);
100+
control_law_->setSpeedLimit(v_linear_min_, v_linear_max_, v_angular_max_);
101+
}
102+
}
103+
104+
result.successful = true;
105+
return result;
106+
}
107+
64108
} // namespace opennav_docking

‎nav2_docking/opennav_docking/test/test_controller.cpp‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,4 +45,38 @@ TEST(ControllerTests, ObjectLifecycle)
4545
controller.reset();
4646
}
4747

48+
TEST(ControllerTests, DynamicParameters) {
49+
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
50+
auto controller = std::make_shared<opennav_docking::Controller>(node);
51+
52+
auto params = std::make_shared<rclcpp::AsyncParametersClient>(
53+
node->get_node_base_interface(), node->get_node_topics_interface(),
54+
node->get_node_graph_interface(),
55+
node->get_node_services_interface());
56+
57+
// Set parameters
58+
auto results = params->set_parameters_atomically(
59+
{rclcpp::Parameter("controller.k_phi", 1.0),
60+
rclcpp::Parameter("controller.k_delta", 2.0),
61+
rclcpp::Parameter("controller.beta", 3.0),
62+
rclcpp::Parameter("controller.lambda", 4.0),
63+
rclcpp::Parameter("controller.v_linear_min", 5.0),
64+
rclcpp::Parameter("controller.v_linear_max", 6.0),
65+
rclcpp::Parameter("controller.v_angular_max", 7.0),
66+
rclcpp::Parameter("controller.slowdown_radius", 8.0)});
67+
68+
// Spin
69+
rclcpp::spin_until_future_complete(node->get_node_base_interface(), results);
70+
71+
// Check parameters
72+
EXPECT_EQ(node->get_parameter("controller.k_phi").as_double(), 1.0);
73+
EXPECT_EQ(node->get_parameter("controller.k_delta").as_double(), 2.0);
74+
EXPECT_EQ(node->get_parameter("controller.beta").as_double(), 3.0);
75+
EXPECT_EQ(node->get_parameter("controller.lambda").as_double(), 4.0);
76+
EXPECT_EQ(node->get_parameter("controller.v_linear_min").as_double(), 5.0);
77+
EXPECT_EQ(node->get_parameter("controller.v_linear_max").as_double(), 6.0);
78+
EXPECT_EQ(node->get_parameter("controller.v_angular_max").as_double(), 7.0);
79+
EXPECT_EQ(node->get_parameter("controller.slowdown_radius").as_double(), 8.0);
80+
}
81+
4882
} // namespace opennav_docking

0 commit comments

Comments
 (0)