Dynamic reconfigure for AMCL

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -27,6 +27,7 @@
 #include <string>
 #include <utility>
 #include <vector>
+#include <mutex>
 
 #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<rclcpp::Parameter> 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<bool> active_{false};

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
@@ -278,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();
 
@@ -295,6 +303,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();
 
@@ -1113,6 +1124,122 @@ AmclNode::initParameters()
   }
 }
 
+/**
+  * @brief Callback executed when a parameter change is detected
+  * @param event ParameterEvent message
+  */
+rcl_interfaces::msg::SetParametersResult
+AmclNode::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  std::lock_guard<std::mutex> lock(pf_mutex_);
+  rcl_interfaces::msg::SetParametersResult result;
+  double save_pose_rate;
+  double tmp_tol;
+
+  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_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") {
+        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") {
+        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 == "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") {
+        odom_frame_id_ = parameter.as_string();
+      } else if (param_name == "scan_topic") {
+        scan_topic_ = parameter.as_string();
+      } else if (param_name == "robot_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") {
+        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();
+      }
+    }
+  }
+
+  result.successful = true;
+  return result;
+}
+
 void
 AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
 {