Skip to content

Commit 47327a5

Browse files
mini-1235mini-1235
authored
Redesign AMCL dynamic parameters pattern (#5621)
* Redesign amcl dynamic parameters patterns Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Linting Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Activate dynamic parameters callback in Avtivate stage Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add activate() and deactivate() in param handler Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Add tests for AMCL Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com>
1 parent 00dfd67 commit 47327a5

File tree

11 files changed

+298
-79
lines changed

11 files changed

+298
-79
lines changed

‎nav2_amcl/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -160,6 +160,12 @@ if(BUILD_TESTING)
160160
set(ament_cmake_cpplint_FOUND TRUE)
161161

162162
ament_lint_auto_find_test_dependencies()
163+
164+
find_package(ament_cmake_gtest REQUIRED)
165+
166+
ament_find_gtest()
167+
168+
add_subdirectory(test)
163169
endif()
164170

165171
ament_export_include_directories("include/${PROJECT_NAME}")

‎nav2_amcl/include/nav2_amcl/amcl_node.hpp‎

Lines changed: 18 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -92,14 +92,27 @@ class AmclNode : public nav2::LifecycleNode
9292
nav2::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
9393

9494
/**
95-
* @brief Callback executed when a parameter change is detected
96-
* @param event ParameterEvent message
95+
* @brief Validate incoming parameter updates before applying them.
96+
* This callback is triggered when one or more parameters are about to be updated.
97+
* It checks the validity of parameter values and rejects updates that would lead
98+
* to invalid or inconsistent configurations
99+
* @param parameters List of parameters that are being updated.
100+
* @return rcl_interfaces::msg::SetParametersResult Result indicating whether the update is accepted.
101+
*/
102+
rcl_interfaces::msg::SetParametersResult validateParameterUpdatesCallback(
103+
std::vector<rclcpp::Parameter> parameters);
104+
105+
/**
106+
* @brief Apply parameter updates after validation
107+
* This callback is executed when parameters have been successfully updated.
108+
* It updates the internal configuration of the node with the new parameter values.
109+
* @param parameters List of parameters that have been updated.
97110
*/
98-
rcl_interfaces::msg::SetParametersResult
99-
dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
111+
void updateParametersCallback(std::vector<rclcpp::Parameter> parameters);
100112

101113
// Dynamic parameters handler
102-
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
114+
rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_;
115+
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_;
103116

104117
// Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't
105118
// respond until we're in the active state

‎nav2_amcl/src/amcl_node.cpp‎

Lines changed: 63 additions & 70 deletions
Original file line numberDiff line numberDiff line change
@@ -116,9 +116,13 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/)
116116

117117
auto node = shared_from_this();
118118
// Add callback for dynamic parameters
119-
dyn_params_handler_ = node->add_on_set_parameters_callback(
119+
post_set_params_handler_ = node->add_post_set_parameters_callback(
120120
std::bind(
121-
&AmclNode::dynamicParametersCallback,
121+
&AmclNode::updateParametersCallback,
122+
this, std::placeholders::_1));
123+
on_set_params_handler_ = node->add_on_set_parameters_callback(
124+
std::bind(
125+
&AmclNode::validateParameterUpdatesCallback,
122126
this, std::placeholders::_1));
123127

124128
// create bond connection
@@ -139,8 +143,10 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
139143
particle_cloud_pub_->on_deactivate();
140144

141145
// shutdown and reset dynamic parameter handler
142-
remove_on_set_parameters_callback(dyn_params_handler_.get());
143-
dyn_params_handler_.reset();
146+
remove_post_set_parameters_callback(post_set_params_handler_.get());
147+
post_set_params_handler_.reset();
148+
remove_on_set_parameters_callback(on_set_params_handler_.get());
149+
on_set_params_handler_.reset();
144150

145151
// destroy bond connection
146152
destroyBond();
@@ -986,84 +992,87 @@ AmclNode::initParameters()
986992
}
987993
}
988994

989-
/**
990-
* @brief Callback executed when a parameter change is detected
991-
* @param event ParameterEvent message
992-
*/
993-
rcl_interfaces::msg::SetParametersResult
994-
AmclNode::dynamicParametersCallback(
995+
rcl_interfaces::msg::SetParametersResult AmclNode::validateParameterUpdatesCallback(
995996
std::vector<rclcpp::Parameter> parameters)
996997
{
997-
std::lock_guard<std::recursive_mutex> cfl(mutex_);
998998
rcl_interfaces::msg::SetParametersResult result;
999-
double save_pose_rate;
1000-
double tmp_tol;
999+
result.successful = true;
1000+
for (auto parameter : parameters) {
1001+
const auto & param_type = parameter.get_type();
1002+
const auto & param_name = parameter.get_name();
1003+
if (param_name.find('.') != std::string::npos) {
1004+
continue;
1005+
}
1006+
if (param_type == ParameterType::PARAMETER_DOUBLE) {
1007+
if (parameter.as_double() < 0.0 &&
1008+
(param_name != "laser_min_range" || param_name != "laser_max_range"))
1009+
{
1010+
RCLCPP_WARN(
1011+
get_logger(), "The value of parameter '%s' is incorrectly set to %f, "
1012+
"it should be >=0. Ignoring parameter update.",
1013+
param_name.c_str(), parameter.as_double());
1014+
result.successful = false;
1015+
}
1016+
} else if (param_type == ParameterType::PARAMETER_INTEGER) {
1017+
if (parameter.as_int() <= 0.0 && param_name == "resample_interval") {
1018+
RCLCPP_WARN(
1019+
get_logger(), "The value of resample_interval is incorrectly set, "
1020+
"it should be >0. Ignoring parameter update.");
1021+
result.successful = false;
1022+
} else if (parameter.as_int() < 0.0) {
1023+
RCLCPP_WARN(
1024+
get_logger(), "The value of parameter '%s' is incorrectly set to %ld, "
1025+
"it should be >=0. Ignoring parameter update.",
1026+
param_name.c_str(), parameter.as_int());
1027+
result.successful = false;
1028+
} else if (param_name == "max_particles" && parameter.as_int() < min_particles_) {
1029+
RCLCPP_WARN(
1030+
get_logger(), "The value of max_particles is incorrectly set, "
1031+
"it should be larger than min_particles. Ignoring parameter update.");
1032+
result.successful = false;
1033+
} else if (param_name == "min_particles" && parameter.as_int() > max_particles_) {
1034+
RCLCPP_WARN(
1035+
get_logger(), "The value of min_particles is incorrectly set, "
1036+
"it should be smaller than max particles. Ignoring parameter update.");
1037+
result.successful = false;
1038+
}
1039+
}
1040+
}
1041+
return result;
1042+
}
10011043

1002-
int max_particles = max_particles_;
1003-
int min_particles = min_particles_;
1044+
void
1045+
AmclNode::updateParametersCallback(
1046+
std::vector<rclcpp::Parameter> parameters)
1047+
{
1048+
std::lock_guard<std::recursive_mutex> cfl(mutex_);
10041049

10051050
bool reinit_pf = false;
10061051
bool reinit_odom = false;
10071052
bool reinit_laser = false;
10081053
bool reinit_map = false;
10091054

1010-
for (auto parameter : parameters) {
1055+
for (const auto & parameter : parameters) {
10111056
const auto & param_type = parameter.get_type();
10121057
const auto & param_name = parameter.get_name();
10131058
if (param_name.find('.') != std::string::npos) {
10141059
continue;
10151060
}
1016-
10171061
if (param_type == ParameterType::PARAMETER_DOUBLE) {
10181062
if (param_name == "alpha1") {
10191063
alpha1_ = parameter.as_double();
1020-
// alpha restricted to be non-negative
1021-
if (alpha1_ < 0.0) {
1022-
RCLCPP_WARN(
1023-
get_logger(), "You've set alpha1 to be negative,"
1024-
" this isn't allowed, so the alpha1 will be set to be zero.");
1025-
alpha1_ = 0.0;
1026-
}
10271064
reinit_odom = true;
10281065
} else if (param_name == "alpha2") {
10291066
alpha2_ = parameter.as_double();
1030-
// alpha restricted to be non-negative
1031-
if (alpha2_ < 0.0) {
1032-
RCLCPP_WARN(
1033-
get_logger(), "You've set alpha2 to be negative,"
1034-
" this isn't allowed, so the alpha2 will be set to be zero.");
1035-
alpha2_ = 0.0;
1036-
}
10371067
reinit_odom = true;
10381068
} else if (param_name == "alpha3") {
10391069
alpha3_ = parameter.as_double();
1040-
// alpha restricted to be non-negative
1041-
if (alpha3_ < 0.0) {
1042-
RCLCPP_WARN(
1043-
get_logger(), "You've set alpha3 to be negative,"
1044-
" this isn't allowed, so the alpha3 will be set to be zero.");
1045-
alpha3_ = 0.0;
1046-
}
10471070
reinit_odom = true;
10481071
} else if (param_name == "alpha4") {
10491072
alpha4_ = parameter.as_double();
1050-
// alpha restricted to be non-negative
1051-
if (alpha4_ < 0.0) {
1052-
RCLCPP_WARN(
1053-
get_logger(), "You've set alpha4 to be negative,"
1054-
" this isn't allowed, so the alpha4 will be set to be zero.");
1055-
alpha4_ = 0.0;
1056-
}
10571073
reinit_odom = true;
10581074
} else if (param_name == "alpha5") {
10591075
alpha5_ = parameter.as_double();
1060-
// alpha restricted to be non-negative
1061-
if (alpha5_ < 0.0) {
1062-
RCLCPP_WARN(
1063-
get_logger(), "You've set alpha5 to be negative,"
1064-
" this isn't allowed, so the alpha5 will be set to be zero.");
1065-
alpha5_ = 0.0;
1066-
}
10671076
reinit_odom = true;
10681077
} else if (param_name == "beam_skip_distance") {
10691078
beam_skip_distance_ = parameter.as_double();
@@ -1099,13 +1108,13 @@ AmclNode::dynamicParametersCallback(
10991108
alpha_slow_ = parameter.as_double();
11001109
reinit_pf = true;
11011110
} else if (param_name == "save_pose_rate") {
1102-
save_pose_rate = parameter.as_double();
1111+
double save_pose_rate = parameter.as_double();
11031112
save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate);
11041113
} else if (param_name == "sigma_hit") {
11051114
sigma_hit_ = parameter.as_double();
11061115
reinit_laser = true;
11071116
} else if (param_name == "transform_tolerance") {
1108-
tmp_tol = parameter.as_double();
1117+
double tmp_tol = parameter.as_double();
11091118
transform_tolerance_ = tf2::durationFromSec(tmp_tol);
11101119
reinit_laser = true;
11111120
} else if (param_name == "update_min_a") {
@@ -1173,19 +1182,6 @@ AmclNode::dynamicParametersCallback(
11731182
}
11741183
}
11751184

1176-
// Checking if the minimum particles is greater than max_particles_
1177-
if (min_particles_ > max_particles_) {
1178-
RCLCPP_ERROR(
1179-
this->get_logger(),
1180-
"You've set min_particles to be greater than max particles,"
1181-
" this isn't allowed.");
1182-
// sticking to the old values
1183-
max_particles_ = max_particles;
1184-
min_particles_ = min_particles;
1185-
result.successful = false;
1186-
return result;
1187-
}
1188-
11891185
// Re-initialize the particle filter
11901186
if (reinit_pf) {
11911187
if (pf_ != NULL) {
@@ -1221,9 +1217,6 @@ AmclNode::dynamicParametersCallback(
12211217
std::bind(&AmclNode::mapReceived, this, std::placeholders::_1),
12221218
nav2::qos::LatchedSubscriptionQoS());
12231219
}
1224-
1225-
result.successful = true;
1226-
return result;
12271220
}
12281221

12291222
void

‎nav2_amcl/test/CMakeLists.txt‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
ament_add_gtest(test_dynamic_parameters
2+
test_dynamic_parameters.cpp
3+
)
4+
target_link_libraries(test_dynamic_parameters
5+
${library_name}
6+
nav2_util::nav2_util_core
7+
rclcpp::rclcpp
8+
)

0 commit comments

Comments
 (0)