Dynamic reconfigure for AMCL

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -91,6 +91,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();
 
@@ -1114,6 +1125,206 @@ 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::recursive_mutex> 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<nav_msgs::msg::OccupancyGrid>(
+      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)
 {