From 9b799328a210aebe2ce00cd9ac00dea4ca7f7f68 Mon Sep 17 00:00:00 2001 From: Thomas Lipps Date: Wed, 27 Sep 2023 15:46:09 +0200 Subject: [PATCH] [nav2_amcl] adding dynamic parameters from PR 2932 --- nav2_amcl/include/nav2_amcl/amcl_node.hpp | 10 + nav2_amcl/src/amcl_node.cpp | 211 ++++++++++++++++++++++ 2 files changed, 221 insertions(+) diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 65b40ddb9f..6c7496ccbc 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -89,6 +89,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 8be37dbeb1..98ace7aa34 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -51,6 +51,7 @@ #include "portable_utils.h" using namespace std::placeholders; +using rcl_interfaces::msg::ParameterType; using namespace std::chrono_literals; namespace nav2_amcl @@ -297,6 +298,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(); @@ -314,6 +322,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(); @@ -1153,6 +1164,206 @@ AmclNode::initParameters() } } +/** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ +rcl_interfaces::msg::SetParametersResult +AmclNode::dynamicParametersCallback( + std::vector parameters) +{ + std::lock_guard cfl(mutex_); + rcl_interfaces::msg::SetParametersResult result; + 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; + bool reinit_map = false; + + 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(); + 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") { + base_frame_id_ = parameter.as_string(); + } else if (param_name == "global_frame_id") { + 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; + } 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") { + set_initial_pose_ = parameter.as_bool(); + } else if (param_name == "first_map_only") { + first_map_only_ = parameter.as_bool(); + } + } 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(); + } + } + } + + // Checking if the minimum particles is greater than max_particles_ + if (min_particles_ > max_particles_) { + RCLCPP_ERROR( + this->get_logger(), + "You've set min_particles to be greater than max particles," + " this isn't allowed."); + // sticking to the old values + max_particles_ = max_particles; + min_particles_ = min_particles; + result.successful = false; + return result; + } + + // Re-initialize the particle filter + if (reinit_pf) { + if (pf_ != NULL) { + pf_free(pf_); + pf_ = NULL; + } + initParticleFilter(); + } + + // Re-initialize the odometry + if (reinit_odom) { + motion_model_.reset(); + initOdometry(); + } + + // Re-initialize the lasers and it's filters + if (reinit_laser) { + lasers_.clear(); + lasers_update_.clear(); + frame_to_laser_.clear(); + laser_scan_connection_.disconnect(); + laser_scan_sub_.reset(); + + initMessageFilters(); + } + + // Re-initialize the map + if (reinit_map) { + map_sub_.reset(); + 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; + return result; +} + void AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg) {