From 9eaeee086fe16f5590d5134cb83dc7587484fcc1 Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Thu, 14 Apr 2022 18:11:48 +0200 Subject: [PATCH 01/13] not finished --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 11 ++ nav2_amcl/src/amcl_node.cpp | 124 ++++++++++++++++++++++ 2 files changed, 135 insertions(+) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index e2446e0c7b..caae631a0b 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include "geometry_msgs/msg/pose_stamped.hpp" #include "message_filters/subscriber.h" @@ -91,6 +92,16 @@ class AmclNode : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + // Since the sensor data from gazebo or the robot is not lifecycle enabled, we won't // respond until we're in the active state std::atomic active_{false}; diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index b9770285bf..9c0ae46410 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -51,6 +51,7 @@ #include "nav2_amcl/portable_utils.hpp" using namespace std::placeholders; +using rcl_interfaces::msg::ParameterType; using namespace std::chrono_literals; namespace nav2_amcl @@ -295,6 +296,9 @@ AmclNode::on_deactivate(const rclcpp_lifecycle::State & /*state*/) pose_pub_->on_deactivate(); particle_cloud_pub_->on_deactivate(); + // reset dynamic parameter handler + dyn_params_handler_.reset(); + // destroy bond connection destroyBond(); @@ -1111,6 +1115,126 @@ AmclNode::initParameters() if (always_reset_initial_pose_) { initial_pose_is_known_ = false; } + + // Add callback for dynamic parameters + dyn_params_handler_ = this->add_on_set_parameters_callback( + std::bind( + &AmclNode::dynamicParametersCallback, + this, std::placeholders::_1)); +} + +/** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ +rcl_interfaces::msg::SetParametersResult +AmclNode::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + + for (auto parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == "alpha1") { + alpha1_ = parameter.as_double(); + } else if (param_name == "alpha2") { + alpha2_ = parameter.as_double(); + } else if (param_name == "alpha3") { + alpha3_ = parameter.as_double(); + } else if (param_name == "alpha4") { + alpha4_ = parameter.as_double(); + } else if (param_name == "alpha5") { + alpha5_ = parameter.as_double(); + } else if (param_name == "beam_skip_distance") { + beam_skip_distance_ = parameter.as_double(); + } else if (param_name == "beam_skip_error_threshold") { + beam_skip_error_threshold_ = parameter.as_double(); + } else if (param_name == "beam_skip_threshold") { + beam_skip_threshold_ = parameter.as_double(); + } else if (param_name == "lambda_short") { + lambda_short_ = parameter.as_double(); + } else if (param_name == "laser_likelihood_max_dist") { + laser_likelihood_max_dist_ = parameter.as_double(); + } else if (param_name == "laser_max_range") { + laser_max_range_ = parameter.as_double(); + } else if (param_name == "laser_min_range") { + laser_min_range_ = parameter.as_double(); + } else if (param_name == "pf_err") { + pf_err_ = parameter.as_double(); + } else if (param_name == "pf_z") { + pf_z_ = parameter.as_double(); + } else if (param_name == "recovery_alpha_fast") { + alpha_fast_ = parameter.as_double(); + } else if (param_name == "recovery_alpha_slow") { + alpha_slow_ = parameter.as_double(); + } else if (param_name == "save_pose_rate") { + save_pose_period_ = parameter.as_double(); + } else if (param_name == "sigma_hit") { + sigma_hit_ = parameter.as_double(); + } else if (param_name == "transform_tolerance") { + transform_tolerance_ = parameter.as_double(); + } else if (param_name == "update_min_a") { + a_thresh_ = parameter.as_double(); + } else if (param_name == "update_min_d") { + d_thresh_ = parameter.as_double(); + } else if (param_name == "z_hit") { + z_hit_ = parameter.as_double(); + } else if (param_name == "z_max") { + z_max_ = parameter.as_double(); + } else if (param_name == "z_rand") { + z_rand_ = parameter.as_double(); + } else if (param_name == "z_short") { + z_short_ = parameter.as_double(); + } + } else if (param_type == ParameterType::PARAMETER_STRING) { + if (param_name == "base_frame_id") { + base_frame_id_ = parameter.as_string(); + } else if (param_name == "global_frame_id") { + global_frame_id_ = parameter.as_string(); + } else if (param_name == "base_frame_id") { + base_frame_id_ = parameter.as_string(); + } else if (param_name == "laser_model_type") { + if (parameter.as_string() == "beam") { + sensor_model_type_ = LASER_MODEL_BEAM; + } else if (parameter.as_string() == "laser_model_type") { + sensor_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD; + } else if (parameter.as_string() == "laser_model_type") { + sensor_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD_PROB; + } else { + RCLCPP_WARN( + get_logger(), "Unknown laser model type \"%s\"; defaulting to likelihood_field model", + tmp_model_type.c_str()); + sensor_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD; + } + } else if (param_name == "odom_frame_id") { + odom_frame_id_ = parameter.as_string(); + } else if (param_name == "scan_topic") { + scan_topic_ = parameter.as_string(); + } else if (param_name == "odom_model_type") { + if (parameter.as_string() == "diff") { + robot_model_type_ = ODOM_MODEL_DIFF; + } else if (parameter.as_string() == "omni") { + robot_model_type_ = ODOM_MODEL_OMNI; + } else if (parameter.as_string() == "diff-corrected") { + robot_model_type_ = ODOM_MODEL_DIFF_CORRECTED; + } else if (parameter.as_string() == "omni-corrected") { + robot_model_type_ = ODOM_MODEL_OMNI_CORRECTED; + } + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == "do_beamskip") { + do_beamskip_ = parameter.as_bool(); + } else if (param_name == "tf_broadcast") { + tf_broadcast_ = parameter.as_bool(); + } + } + } + + result.successful = true; + return result; } void From 6bda2a380977cc8d8b0ba5004fb3f69e75f07a3d Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Tue, 3 May 2022 11:29:42 +0200 Subject: [PATCH 02/13] update --- nav2_amcl/src/amcl_node.cpp | 43 ++++++++++++------------------------- 1 file changed, 14 insertions(+), 29 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 9c0ae46410..3c1776833b 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1132,6 +1132,8 @@ AmclNode::dynamicParametersCallback( std::vector parameters) { rcl_interfaces::msg::SetParametersResult result; + double save_pose_rate; + double tmp_tol; for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); @@ -1171,11 +1173,13 @@ AmclNode::dynamicParametersCallback( } else if (param_name == "recovery_alpha_slow") { alpha_slow_ = parameter.as_double(); } else if (param_name == "save_pose_rate") { - save_pose_period_ = parameter.as_double(); + save_pose_rate = parameter.as_double(); + save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); } else if (param_name == "sigma_hit") { sigma_hit_ = parameter.as_double(); } else if (param_name == "transform_tolerance") { - transform_tolerance_ = parameter.as_double(); + tmp_tol = parameter.as_double(); + transform_tolerance_ = tf2::durationFromSec(tmp_tol); } else if (param_name == "update_min_a") { a_thresh_ = parameter.as_double(); } else if (param_name == "update_min_d") { @@ -1197,34 +1201,15 @@ AmclNode::dynamicParametersCallback( } else if (param_name == "base_frame_id") { base_frame_id_ = parameter.as_string(); } else if (param_name == "laser_model_type") { - if (parameter.as_string() == "beam") { - sensor_model_type_ = LASER_MODEL_BEAM; - } else if (parameter.as_string() == "laser_model_type") { - sensor_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD; - } else if (parameter.as_string() == "laser_model_type") { - sensor_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD_PROB; - } else { - RCLCPP_WARN( - get_logger(), "Unknown laser model type \"%s\"; defaulting to likelihood_field model", - tmp_model_type.c_str()); - sensor_model_type_ = LASER_MODEL_LIKELIHOOD_FIELD; - } - } else if (param_name == "odom_frame_id") { + sensor_model_type_ = parameter.as_string(); + } else if (param_name == "odom_frame_id") { odom_frame_id_ = parameter.as_string(); - } else if (param_name == "scan_topic") { - scan_topic_ = parameter.as_string(); - } else if (param_name == "odom_model_type") { - if (parameter.as_string() == "diff") { - robot_model_type_ = ODOM_MODEL_DIFF; - } else if (parameter.as_string() == "omni") { - robot_model_type_ = ODOM_MODEL_OMNI; - } else if (parameter.as_string() == "diff-corrected") { - robot_model_type_ = ODOM_MODEL_DIFF_CORRECTED; - } else if (parameter.as_string() == "omni-corrected") { - robot_model_type_ = ODOM_MODEL_OMNI_CORRECTED; - } - } - } else if (param_type == ParameterType::PARAMETER_BOOL) { + } else if (param_name == "scan_topic") { + scan_topic_ = parameter.as_string(); + } else if (param_name == "odom_model_type") { + robot_model_type_ = parameter.as_string(); + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == "do_beamskip") { do_beamskip_ = parameter.as_bool(); } else if (param_name == "tf_broadcast") { From 6325f024c1b8266127f4cbe0c548be1b48786008 Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Tue, 3 May 2022 12:53:02 +0200 Subject: [PATCH 03/13] satisfying linters --- nav2_amcl/src/amcl_node.cpp | 86 ++++++++++++++++++++++--------------- 1 file changed, 51 insertions(+), 35 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 3c1776833b..c60f2620a8 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1142,78 +1142,94 @@ AmclNode::dynamicParametersCallback( if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == "alpha1") { alpha1_ = parameter.as_double(); - } else if (param_name == "alpha2") { + } else if (param_name == "alpha2") { alpha2_ = parameter.as_double(); - } else if (param_name == "alpha3") { + } else if (param_name == "alpha3") { alpha3_ = parameter.as_double(); - } else if (param_name == "alpha4") { + } else if (param_name == "alpha4") { alpha4_ = parameter.as_double(); - } else if (param_name == "alpha5") { + } else if (param_name == "alpha5") { alpha5_ = parameter.as_double(); - } else if (param_name == "beam_skip_distance") { + } else if (param_name == "beam_skip_distance") { beam_skip_distance_ = parameter.as_double(); - } else if (param_name == "beam_skip_error_threshold") { + } else if (param_name == "beam_skip_error_threshold") { beam_skip_error_threshold_ = parameter.as_double(); - } else if (param_name == "beam_skip_threshold") { + } else if (param_name == "beam_skip_threshold") { beam_skip_threshold_ = parameter.as_double(); - } else if (param_name == "lambda_short") { + } else if (param_name == "lambda_short") { lambda_short_ = parameter.as_double(); - } else if (param_name == "laser_likelihood_max_dist") { + } else if (param_name == "laser_likelihood_max_dist") { laser_likelihood_max_dist_ = parameter.as_double(); - } else if (param_name == "laser_max_range") { + } else if (param_name == "laser_max_range") { laser_max_range_ = parameter.as_double(); - } else if (param_name == "laser_min_range") { + } else if (param_name == "laser_min_range") { laser_min_range_ = parameter.as_double(); - } else if (param_name == "pf_err") { + } else if (param_name == "pf_err") { pf_err_ = parameter.as_double(); - } else if (param_name == "pf_z") { + } else if (param_name == "pf_z") { pf_z_ = parameter.as_double(); - } else if (param_name == "recovery_alpha_fast") { + } else if (param_name == "recovery_alpha_fast") { alpha_fast_ = parameter.as_double(); - } else if (param_name == "recovery_alpha_slow") { + } else if (param_name == "recovery_alpha_slow") { alpha_slow_ = parameter.as_double(); - } else if (param_name == "save_pose_rate") { + } else if (param_name == "save_pose_rate") { save_pose_rate = parameter.as_double(); save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); - } else if (param_name == "sigma_hit") { + } else if (param_name == "sigma_hit") { sigma_hit_ = parameter.as_double(); - } else if (param_name == "transform_tolerance") { + } else if (param_name == "transform_tolerance") { tmp_tol = parameter.as_double(); transform_tolerance_ = tf2::durationFromSec(tmp_tol); - } else if (param_name == "update_min_a") { + } else if (param_name == "update_min_a") { a_thresh_ = parameter.as_double(); - } else if (param_name == "update_min_d") { + } else if (param_name == "update_min_d") { d_thresh_ = parameter.as_double(); - } else if (param_name == "z_hit") { + } else if (param_name == "z_hit") { z_hit_ = parameter.as_double(); - } else if (param_name == "z_max") { + } else if (param_name == "z_max") { z_max_ = parameter.as_double(); - } else if (param_name == "z_rand") { + } else if (param_name == "z_rand") { z_rand_ = parameter.as_double(); - } else if (param_name == "z_short") { + } else if (param_name == "z_short") { z_short_ = parameter.as_double(); - } - } else if (param_type == ParameterType::PARAMETER_STRING) { + } + } else if (param_type == ParameterType::PARAMETER_STRING) { if (param_name == "base_frame_id") { base_frame_id_ = parameter.as_string(); - } else if (param_name == "global_frame_id") { + } else if (param_name == "global_frame_id") { global_frame_id_ = parameter.as_string(); - } else if (param_name == "base_frame_id") { - base_frame_id_ = parameter.as_string(); - } else if (param_name == "laser_model_type") { + } else if (param_name == "map_topic") { + map_topic_ = parameter.as_string(); + } else if (param_name == "laser_model_type") { sensor_model_type_ = parameter.as_string(); - } else if (param_name == "odom_frame_id") { + } else if (param_name == "odom_frame_id") { odom_frame_id_ = parameter.as_string(); - } else if (param_name == "scan_topic") { + } else if (param_name == "scan_topic") { scan_topic_ = parameter.as_string(); - } else if (param_name == "odom_model_type") { + } else if (param_name == "robot_model_type") { robot_model_type_ = parameter.as_string(); - } - } else if (param_type == ParameterType::PARAMETER_BOOL) { + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == "do_beamskip") { do_beamskip_ = parameter.as_bool(); } else if (param_name == "tf_broadcast") { tf_broadcast_ = parameter.as_bool(); + } else if (param_name == "set_initial_pose") { + tf_broadcast_ = parameter.as_bool(); + } else if (param_name == "always_reset_initial_pose") { + tf_broadcast_ = parameter.as_bool(); + } else if (param_name == "first_map_only") { + tf_broadcast_ = parameter.as_bool(); + } + } else if (param_type == ParameterType::PARAMETER_INTEGER) { + if (param_name == "max_beams") { + max_beams_ = parameter.as_int(); + } else if (param_name == "max_particles") { + max_particles_ = parameter.as_int(); + } else if (param_name == "min_particles") { + min_particles_ = parameter.as_int(); + } else if (param_name == "resample_interval") { + resample_interval_ = parameter.as_int(); } } } From 350fd15c3270e563413768fb397b2745670f4669 Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Tue, 3 May 2022 13:43:21 +0200 Subject: [PATCH 04/13] add mutex --- nav2_amcl/src/amcl_node.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index c60f2620a8..833947e5cb 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -279,6 +279,13 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/) handleInitialPose(last_published_pose_); } + auto node = shared_from_this(); + // Add callback for dynamic parameters + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &AmclNode::dynamicParametersCallback, + this, std::placeholders::_1)); + // create bond connection createBond(); @@ -1115,12 +1122,6 @@ AmclNode::initParameters() if (always_reset_initial_pose_) { initial_pose_is_known_ = false; } - - // Add callback for dynamic parameters - dyn_params_handler_ = this->add_on_set_parameters_callback( - std::bind( - &AmclNode::dynamicParametersCallback, - this, std::placeholders::_1)); } /** @@ -1131,6 +1132,7 @@ rcl_interfaces::msg::SetParametersResult AmclNode::dynamicParametersCallback( std::vector parameters) { + std::lock_guard lock(pf_mutex_); rcl_interfaces::msg::SetParametersResult result; double save_pose_rate; double tmp_tol; @@ -1440,7 +1442,6 @@ AmclNode::initParticleFilter() reinterpret_cast(map_)); pf_->pop_err = pf_err_; pf_->pop_z = pf_z_; - // Initialize the filter pf_vector_t pf_init_pose_mean = pf_vector_zero(); pf_init_pose_mean.v[0] = init_pose_[0]; From e198f6d4f87ae022d634d6ac6bcf183b62804fe8 Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Tue, 3 May 2022 13:45:08 +0200 Subject: [PATCH 05/13] fix mistake --- nav2_amcl/src/amcl_node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 833947e5cb..4a4ae47446 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1442,6 +1442,7 @@ AmclNode::initParticleFilter() reinterpret_cast(map_)); pf_->pop_err = pf_err_; pf_->pop_z = pf_z_; + // Initialize the filter pf_vector_t pf_init_pose_mean = pf_vector_zero(); pf_init_pose_mean.v[0] = init_pose_[0]; From 8bd6175654e90ee882416b575da8a9e00d65526f Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Wed, 4 May 2022 11:53:06 +0200 Subject: [PATCH 06/13] dealing each parameters --- nav2_amcl/src/amcl_node.cpp | 41 +++++++++++++++++++++++++++++++++---- 1 file changed, 37 insertions(+), 4 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 4a4ae47446..a68450d679 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1132,7 +1132,7 @@ rcl_interfaces::msg::SetParametersResult AmclNode::dynamicParametersCallback( std::vector parameters) { - std::lock_guard lock(pf_mutex_); + std::lock_guard cfl(configuration_mutex_); rcl_interfaces::msg::SetParametersResult result; double save_pose_rate; double tmp_tol; @@ -1217,11 +1217,11 @@ AmclNode::dynamicParametersCallback( } else if (param_name == "tf_broadcast") { tf_broadcast_ = parameter.as_bool(); } else if (param_name == "set_initial_pose") { - tf_broadcast_ = parameter.as_bool(); + set_initial_pose_ = parameter.as_bool(); } else if (param_name == "always_reset_initial_pose") { - tf_broadcast_ = parameter.as_bool(); + always_reset_initial_pose_ = parameter.as_bool(); } else if (param_name == "first_map_only") { - tf_broadcast_ = parameter.as_bool(); + first_map_only_ = parameter.as_bool(); } } else if (param_type == ParameterType::PARAMETER_INTEGER) { if (param_name == "max_beams") { @@ -1236,6 +1236,39 @@ AmclNode::dynamicParametersCallback( } } + // Checking if the minimum particles is greater than max_particles_ + if (min_particles_ > max_particles_) { + RCLCPP_WARN( + rclcpp_node_->get_logger(), + "You've set min_particles to be greater than max particles," + "this isn't allowed so they'll be set to be equal."); + max_particles_ = min_particles_; + } + + // Re-initialize the particle filter + if (pf_ != NULL) { + pf_free(pf_); + pf_ = NULL; + } + initParticleFilter(); + + // Re-initialize the odometry + motion_model_.reset(); + initOdometry(); + + // Re-initialize the lasers and it's filters + lasers_.clear(); + lasers_update_.clear(); + frame_to_laser_.clear(); + + laser_scan_filter_ = std::make_unique>( + *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_); + + laser_scan_connection_ = laser_scan_filter_->registerCallback( + std::bind( + &AmclNode::laserReceived, + this, std::placeholders::_1)); + result.successful = true; return result; } From e379d7b585818f5f171c20ee4ebe26887d7203ee Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Wed, 4 May 2022 12:55:14 +0200 Subject: [PATCH 07/13] segregating variables and instantiating the objects correspondingly --- nav2_amcl/src/amcl_node.cpp | 76 ++++++++++++++++++++++++++++--------- 1 file changed, 58 insertions(+), 18 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index a68450d679..abc053b917 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1137,6 +1137,10 @@ AmclNode::dynamicParametersCallback( double save_pose_rate; double tmp_tol; + bool reinit_pf = false; + bool reinit_odom = false; + bool reinit_laser = false; + for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); const auto & param_name = parameter.get_name(); @@ -1144,56 +1148,78 @@ AmclNode::dynamicParametersCallback( if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == "alpha1") { alpha1_ = parameter.as_double(); + reinit_odom = true; } else if (param_name == "alpha2") { alpha2_ = parameter.as_double(); + reinit_odom = true; } else if (param_name == "alpha3") { alpha3_ = parameter.as_double(); + reinit_odom = true; } else if (param_name == "alpha4") { alpha4_ = parameter.as_double(); + reinit_odom = true; } else if (param_name == "alpha5") { alpha5_ = parameter.as_double(); + reinit_odom = true; } else if (param_name == "beam_skip_distance") { beam_skip_distance_ = parameter.as_double(); + reinit_laser = true; } else if (param_name == "beam_skip_error_threshold") { beam_skip_error_threshold_ = parameter.as_double(); + reinit_laser = true; } else if (param_name == "beam_skip_threshold") { beam_skip_threshold_ = parameter.as_double(); + reinit_laser = true; } else if (param_name == "lambda_short") { lambda_short_ = parameter.as_double(); + reinit_laser = true; } else if (param_name == "laser_likelihood_max_dist") { laser_likelihood_max_dist_ = parameter.as_double(); + reinit_laser = true; } else if (param_name == "laser_max_range") { laser_max_range_ = parameter.as_double(); + reinit_laser = true; } else if (param_name == "laser_min_range") { laser_min_range_ = parameter.as_double(); + reinit_laser = true; } else if (param_name == "pf_err") { pf_err_ = parameter.as_double(); + reinit_pf = true; } else if (param_name == "pf_z") { pf_z_ = parameter.as_double(); + reinit_pf = true; } else if (param_name == "recovery_alpha_fast") { alpha_fast_ = parameter.as_double(); + reinit_pf = true; } else if (param_name == "recovery_alpha_slow") { alpha_slow_ = parameter.as_double(); + reinit_pf = true; } else if (param_name == "save_pose_rate") { save_pose_rate = parameter.as_double(); save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); } else if (param_name == "sigma_hit") { sigma_hit_ = parameter.as_double(); + reinit_laser = true; } else if (param_name == "transform_tolerance") { tmp_tol = parameter.as_double(); transform_tolerance_ = tf2::durationFromSec(tmp_tol); + reinit_laser = true; } else if (param_name == "update_min_a") { a_thresh_ = parameter.as_double(); } else if (param_name == "update_min_d") { d_thresh_ = parameter.as_double(); } else if (param_name == "z_hit") { z_hit_ = parameter.as_double(); + reinit_laser = true; } else if (param_name == "z_max") { z_max_ = parameter.as_double(); + reinit_laser = true; } else if (param_name == "z_rand") { z_rand_ = parameter.as_double(); + reinit_laser = true; } else if (param_name == "z_short") { z_short_ = parameter.as_double(); + reinit_laser = true; } } else if (param_type == ParameterType::PARAMETER_STRING) { if (param_name == "base_frame_id") { @@ -1204,16 +1230,21 @@ AmclNode::dynamicParametersCallback( map_topic_ = parameter.as_string(); } else if (param_name == "laser_model_type") { sensor_model_type_ = parameter.as_string(); + reinit_laser = true; } else if (param_name == "odom_frame_id") { odom_frame_id_ = parameter.as_string(); + reinit_laser = true; } else if (param_name == "scan_topic") { scan_topic_ = parameter.as_string(); + reinit_laser = true; } else if (param_name == "robot_model_type") { robot_model_type_ = parameter.as_string(); + reinit_odom = true; } } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == "do_beamskip") { do_beamskip_ = parameter.as_bool(); + reinit_laser = true; } else if (param_name == "tf_broadcast") { tf_broadcast_ = parameter.as_bool(); } else if (param_name == "set_initial_pose") { @@ -1226,10 +1257,13 @@ AmclNode::dynamicParametersCallback( } else if (param_type == ParameterType::PARAMETER_INTEGER) { if (param_name == "max_beams") { max_beams_ = parameter.as_int(); + reinit_laser = true; } else if (param_name == "max_particles") { max_particles_ = parameter.as_int(); + reinit_pf = true; } else if (param_name == "min_particles") { min_particles_ = parameter.as_int(); + reinit_pf = true; } else if (param_name == "resample_interval") { resample_interval_ = parameter.as_int(); } @@ -1239,35 +1273,41 @@ AmclNode::dynamicParametersCallback( // Checking if the minimum particles is greater than max_particles_ if (min_particles_ > max_particles_) { RCLCPP_WARN( - rclcpp_node_->get_logger(), + this->get_logger(), "You've set min_particles to be greater than max particles," "this isn't allowed so they'll be set to be equal."); max_particles_ = min_particles_; } // Re-initialize the particle filter - if (pf_ != NULL) { - pf_free(pf_); - pf_ = NULL; + if (reinit_pf) { + if (pf_ != NULL) { + pf_free(pf_); + pf_ = NULL; + } + initParticleFilter(); } - initParticleFilter(); // Re-initialize the odometry - motion_model_.reset(); - initOdometry(); + if (reinit_odom) { + motion_model_.reset(); + initOdometry(); + } // Re-initialize the lasers and it's filters - lasers_.clear(); - lasers_update_.clear(); - frame_to_laser_.clear(); - - laser_scan_filter_ = std::make_unique>( - *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_); - - laser_scan_connection_ = laser_scan_filter_->registerCallback( - std::bind( - &AmclNode::laserReceived, - this, std::placeholders::_1)); + if (reinit_laser) { + lasers_.clear(); + lasers_update_.clear(); + frame_to_laser_.clear(); + + laser_scan_filter_ = std::make_unique>( + *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_); + + laser_scan_connection_ = laser_scan_filter_->registerCallback( + std::bind( + &AmclNode::laserReceived, + this, std::placeholders::_1)); + } result.successful = true; return result; From 4fcd0224e41d9bec85d5fbab65db28a19888dadc Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Wed, 4 May 2022 16:27:36 +0200 Subject: [PATCH 08/13] remove unwanted mutex --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index caae631a0b..e270d14b98 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -27,7 +27,6 @@ #include #include #include -#include #include "geometry_msgs/msg/pose_stamped.hpp" #include "message_filters/subscriber.h" From b2896ab9a66b440ec78bb60acd9532b7180b0a08 Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Thu, 5 May 2022 10:15:35 +0200 Subject: [PATCH 09/13] reconfigure for map topic --- nav2_amcl/src/amcl_node.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index abc053b917..7ba7967c2c 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1140,6 +1140,7 @@ AmclNode::dynamicParametersCallback( bool reinit_pf = false; bool reinit_odom = false; bool reinit_laser = false; + bool reinit_map = false; for (auto parameter : parameters) { const auto & param_type = parameter.get_type(); @@ -1228,6 +1229,7 @@ AmclNode::dynamicParametersCallback( global_frame_id_ = parameter.as_string(); } else if (param_name == "map_topic") { map_topic_ = parameter.as_string(); + reinit_map = true; } else if (param_name == "laser_model_type") { sensor_model_type_ = parameter.as_string(); reinit_laser = true; @@ -1309,6 +1311,12 @@ AmclNode::dynamicParametersCallback( this, std::placeholders::_1)); } + // Re-initialize the map + if (reinit_map) { + map_sub_.reset(); + initPubSub(); + } + result.successful = true; return result; } From f78072d1901fa2fc124e3bb8819e46f08446913f Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Fri, 6 May 2022 12:42:18 +0200 Subject: [PATCH 10/13] review update --- nav2_amcl/src/amcl_node.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 7ba7967c2c..fdcc9d4217 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1251,8 +1251,6 @@ AmclNode::dynamicParametersCallback( tf_broadcast_ = parameter.as_bool(); } else if (param_name == "set_initial_pose") { set_initial_pose_ = parameter.as_bool(); - } else if (param_name == "always_reset_initial_pose") { - always_reset_initial_pose_ = parameter.as_bool(); } else if (param_name == "first_map_only") { first_map_only_ = parameter.as_bool(); } @@ -1274,11 +1272,13 @@ AmclNode::dynamicParametersCallback( // Checking if the minimum particles is greater than max_particles_ if (min_particles_ > max_particles_) { - RCLCPP_WARN( + RCLCPP_ERROR( this->get_logger(), "You've set min_particles to be greater than max particles," - "this isn't allowed so they'll be set to be equal."); + " this isn't allowed."); max_particles_ = min_particles_; + result.successful = false; + return result; } // Re-initialize the particle filter @@ -1314,7 +1314,9 @@ AmclNode::dynamicParametersCallback( // Re-initialize the map if (reinit_map) { map_sub_.reset(); - initPubSub(); + map_sub_ = create_subscription( + map_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&AmclNode::mapReceived, this, std::placeholders::_1)); } result.successful = true; From 4bd6b6556165c04589e8fdcd863a21029666db97 Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Mon, 9 May 2022 10:48:20 +0200 Subject: [PATCH 11/13] careful updation of min and max particles --- nav2_amcl/src/amcl_node.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index fdcc9d4217..aa41add2ae 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1137,6 +1137,9 @@ AmclNode::dynamicParametersCallback( double save_pose_rate; double tmp_tol; + int max_particles = max_particles_; + int min_particles = min_particles_; + bool reinit_pf = false; bool reinit_odom = false; bool reinit_laser = false; @@ -1276,7 +1279,9 @@ AmclNode::dynamicParametersCallback( this->get_logger(), "You've set min_particles to be greater than max particles," " this isn't allowed."); - max_particles_ = min_particles_; + // sticking to the old values + max_particles_ = max_particles; + min_particles_ = min_particles; result.successful = false; return result; } From 108f8b94949e3d4d2a4f260b8729fdcf4b82d642 Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Mon, 9 May 2022 11:07:58 +0200 Subject: [PATCH 12/13] resetting and reinitilaizing the laser scanner and message filters --- nav2_amcl/src/amcl_node.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index aa41add2ae..c2c596920e 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1306,14 +1306,10 @@ AmclNode::dynamicParametersCallback( lasers_.clear(); lasers_update_.clear(); frame_to_laser_.clear(); + laser_scan_connection_.disconnect(); + laser_scan_sub_.reset(); - laser_scan_filter_ = std::make_unique>( - *laser_scan_sub_, *tf_buffer_, odom_frame_id_, 10, rclcpp_node_, transform_tolerance_); - - laser_scan_connection_ = laser_scan_filter_->registerCallback( - std::bind( - &AmclNode::laserReceived, - this, std::placeholders::_1)); + initMessageFilters(); } // Re-initialize the map From befa7994e83b8c565c6d0f105e372a5cf8de5a2a Mon Sep 17 00:00:00 2001 From: Pradheep-office Date: Mon, 9 May 2022 17:18:45 +0200 Subject: [PATCH 13/13] fixing the mutex --- nav2_amcl/src/amcl_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index b7b0164018..c645737b2a 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1133,7 +1133,7 @@ rcl_interfaces::msg::SetParametersResult AmclNode::dynamicParametersCallback( std::vector parameters) { - std::lock_guard cfl(configuration_mutex_); + std::lock_guard cfl(mutex_); rcl_interfaces::msg::SetParametersResult result; double save_pose_rate; double tmp_tol;