Dynamic parameter setting of costmap layers (inflation/static/voxel)

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -174,6 +174,13 @@ class InflationLayer : public Layer
     unsigned int index, unsigned int mx, unsigned int my,
     unsigned int src_x, unsigned int src_y);
 
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
   double inflation_radius_, inscribed_radius_, cost_scaling_factor_;
   bool inflate_unknown_, inflate_around_unknown_;
   unsigned int cell_inflation_radius_;
@@ -193,6 +200,8 @@ class InflationLayer : public Layer
   // Indicates that the entire costmap should be reinflated next time around.
   bool need_reinflation_;
   mutex_t * access_;
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp

@@ -87,6 +87,13 @@ class StaticLayer : public CostmapLayer
 
   unsigned char interpretValue(unsigned char value);
 
+  /**
+   * @brief Callback executed when a parameter change is detected
+   * @param event ParameterEvent message
+   */
+  rcl_interfaces::msg::SetParametersResult
+  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+
   std::string global_frame_;  ///< @brief The global frame for the costmap
   std::string map_frame_;  /// @brief frame that map is located in
 
@@ -113,6 +120,9 @@ class StaticLayer : public CostmapLayer
   tf2::Duration transform_tolerance_;
   std::atomic<bool> update_in_progress_;
   nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_;
+  // Dynamic parameters handler
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  std::mutex dyn_param_mutex_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp

@@ -51,6 +51,7 @@
 #include <message_filters/subscriber.h>
 #include <nav2_costmap_2d/obstacle_layer.hpp>
 #include <nav2_voxel_grid/voxel_grid.hpp>
+#include <mutex>
 
 namespace nav2_costmap_2d
 {
@@ -156,6 +157,18 @@ class VoxelLayer : public ObstacleLayer
   {
     return (size_z_ - 1 + 0.5) * z_resolution_;
   }
+
+  /**
+   * @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
+  std::mutex mutex_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/plugins/inflation_layer.cpp

@@ -53,6 +53,7 @@ PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::InflationLayer, nav2_costmap_2d::Layer)
 using nav2_costmap_2d::LETHAL_OBSTACLE;
 using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
 using nav2_costmap_2d::NO_INFORMATION;
+using rcl_interfaces::msg::ParameterType;
 
 namespace nav2_costmap_2d
 {
@@ -77,6 +78,7 @@ InflationLayer::InflationLayer()
 
 InflationLayer::~InflationLayer()
 {
+  dyn_params_handler_.reset();
   delete access_;
 }
 
@@ -102,6 +104,11 @@ InflationLayer::onInitialize()
   need_reinflation_ = false;
   cell_inflation_radius_ = cellDistance(inflation_radius_);
   matchSize();
+
+  dyn_params_handler_ = node_->add_on_set_parameters_callback(
+    std::bind(
+      &InflationLayer::dynamicParametersCallback,
+      this, std::placeholders::_1));
 }
 
 void
@@ -388,4 +395,78 @@ InflationLayer::generateIntegerDistances()
   return level;
 }
 
+/**
+  * @brief Callback executed when a parameter change is detected
+  * @param event ParameterEvent message
+  */
+rcl_interfaces::msg::SetParametersResult
+InflationLayer::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+
+  struct paramChange
+  {
+    bool inflation_radius : 1;
+    bool cost_scaling_factor : 1;
+    bool enabled : 1;
+    bool inflate_unknown : 1;
+    bool inflate_around_unknown : 1;
+  } paramChange_s{0, 0, 0, 0, 0};
+
+  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 == name_ + "." + "inflation_radius" &&
+        inflation_radius_ != parameter.as_double())
+      {
+        inflation_radius_ = parameter.as_double();
+        paramChange_s.inflation_radius = true;
+        cell_inflation_radius_ = cellDistance(inflation_radius_);
+
+      } else if (param_name == name_ + "." + "cost_scaling_factor" &&
+        cost_scaling_factor_ != parameter.as_double())
+      {
+        cost_scaling_factor_ = parameter.as_double();
+        paramChange_s.cost_scaling_factor = true;
+      }
+    } else if (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) {
+        enabled_ = parameter.as_bool();
+        paramChange_s.enabled = true;
+        RCLCPP_INFO(node_->get_logger(), "Updated!");
+      } else if (param_name == name_ + "." + "inflate_unknown" &&
+        inflate_unknown_ != parameter.as_bool())
+      {
+        inflate_unknown_ = parameter.as_bool();
+        paramChange_s.inflate_unknown = true;
+      } else if (param_name == name_ + "." + "inflate_around_unknown" &&
+        inflate_around_unknown_ != parameter.as_bool())
+      {
+        inflate_around_unknown_ = parameter.as_bool();
+        paramChange_s.inflate_around_unknown = true;
+      }
+    }
+
+  }
+
+  if (paramChange_s.enabled || paramChange_s.inflate_unknown ||
+    paramChange_s.inflate_around_unknown || paramChange_s.inflation_radius ||
+    paramChange_s.cost_scaling_factor)
+  {
+    need_reinflation_ = true;
+  }
+
+  if (paramChange_s.inflation_radius || paramChange_s.cost_scaling_factor) {
+    computeCaches();
+  }
+
+  result.successful = true;
+  return result;
+}
+
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/plugins/static_layer.cpp

@@ -53,6 +53,7 @@ PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::StaticLayer, nav2_costmap_2d::Layer)
 using nav2_costmap_2d::NO_INFORMATION;
 using nav2_costmap_2d::LETHAL_OBSTACLE;
 using nav2_costmap_2d::FREE_SPACE;
+using rcl_interfaces::msg::ParameterType;
 
 namespace nav2_costmap_2d
 {
@@ -106,6 +107,7 @@ StaticLayer::activate()
 void
 StaticLayer::deactivate()
 {
+  dyn_params_handler_.reset();
 }
 
 void
@@ -152,6 +154,12 @@ StaticLayer::getParameters()
   update_in_progress_.store(false);
 
   transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node_->add_on_set_parameters_callback(
+    std::bind(
+      &StaticLayer::dynamicParametersCallback,
+      this, std::placeholders::_1));
 }
 
 void
@@ -362,6 +370,7 @@ StaticLayer::updateCosts(
   nav2_costmap_2d::Costmap2D & master_grid,
   int min_i, int min_j, int max_i, int max_j)
 {
+  std::lock_guard<std::mutex> lock_reinit(dyn_param_mutex_);
   if (!enabled_) {
     update_in_progress_.store(false);
     return;
@@ -424,4 +433,46 @@ StaticLayer::updateCosts(
   update_in_progress_.store(false);
 }
 
+/**
+  * @brief Callback executed when a parameter change is detected
+  * @param event ParameterEvent message
+  */
+rcl_interfaces::msg::SetParametersResult
+StaticLayer::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  std::lock_guard<std::mutex> lock_reinit(dyn_param_mutex_);
+
+  for (auto parameter : parameters) {
+    const auto & param_type = parameter.get_type();
+    const auto & param_name = parameter.get_name();
+
+    if (param_name == name_ + "." + "map_subscribe_transient_local" ||
+      param_name == name_ + "." + "map_topic" ||
+      param_name == name_ + "." + "subscribe_to_updates")
+    {
+      RCLCPP_WARN(
+        node_->get_logger(), "%s is not a dynamic parameter "
+        "cannot be changed while running. Rejecting parameter update.", param_name.c_str());
+    } else if (param_type == ParameterType::PARAMETER_DOUBLE) {
+      if (param_name == name_ + "." + "transform_tolerance") {
+        transform_tolerance_ = tf2::durationFromSec(parameter.as_double());
+      }
+    } else if (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) {
+        enabled_ = parameter.as_bool();
+
+        x_ = y_ = 0;
+        width_ = size_x_;
+        height_ = size_y_;
+        has_updated_data_ = true;
+      }
+    }
+
+  }
+  result.successful = true;
+  return result;
+}
+
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -53,6 +53,7 @@ PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::VoxelLayer, nav2_costmap_2d::Layer)
 using nav2_costmap_2d::NO_INFORMATION;
 using nav2_costmap_2d::LETHAL_OBSTACLE;
 using nav2_costmap_2d::FREE_SPACE;
+using rcl_interfaces::msg::ParameterType;
 
 namespace nav2_costmap_2d
 {
@@ -96,10 +97,17 @@ void VoxelLayer::onInitialize()
 
   unknown_threshold_ += (VOXEL_BITS - size_z_);
   matchSize();
+
+  // Add callback for dynamic parameters
+  dyn_params_handler_ = node_->add_on_set_parameters_callback(
+    std::bind(
+      &VoxelLayer::dynamicParametersCallback,
+      this, std::placeholders::_1));
 }
 
 VoxelLayer::~VoxelLayer()
 {
+  dyn_params_handler_.reset();
 }
 
 void VoxelLayer::matchSize()
@@ -130,6 +138,8 @@ void VoxelLayer::updateBounds(
   double robot_x, double robot_y, double robot_yaw, double * min_x,
   double * min_y, double * max_x, double * max_y)
 {
+  std::lock_guard<std::mutex> lock_reinit(mutex_);
+
   if (rolling_window_) {
     updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
   }
@@ -475,4 +485,60 @@ void VoxelLayer::updateOrigin(double new_origin_x, double new_origin_y)
   delete[] local_voxel_map;
 }
 
+/**
+  * @brief Callback executed when a parameter change is detected
+  * @param event ParameterEvent message
+  */
+rcl_interfaces::msg::SetParametersResult
+VoxelLayer::dynamicParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  std::lock_guard<std::mutex> lock_reinit(mutex_);
+
+  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 == name_ + "." + "max_obstacle_height") {
+        max_obstacle_height_ = parameter.as_double();
+      } else if (param_name == name_ + "." + "origin_z") {
+        origin_z_ = parameter.as_double();
+      } else if (param_name == name_ + "." + "z_resolution") {
+        z_resolution_ = parameter.as_double();
+      }
+    } else if (param_type == ParameterType::PARAMETER_BOOL) {
+      if (param_name == name_ + "." + "enabled") {
+        enabled_ = parameter.as_bool();
+      } else if (param_name == name_ + "." + "footprint_clearing_enabled") {
+        footprint_clearing_enabled_ = parameter.as_bool();
+      } else if (param_name == name_ + "." + "publish_voxel_map") {
+        RCLCPP_WARN(
+          node_->get_logger(), "publish voxel map is not a dynamic parameter "
+          "cannot be changed while running. Rejecting parameter update.");
+        continue;
+      }
+
+    } else if (param_type == ParameterType::PARAMETER_INTEGER) {
+      if (param_name == name_ + "." + "z_voxels") {
+        size_z_ = parameter.as_int();
+      } else if (param_name == name_ + "." + "unknown_threshold") {
+        unknown_threshold_ = parameter.as_int() + (VOXEL_BITS - size_z_);
+      } else if (param_name == name_ + "." + "mark_threshold") {
+        mark_threshold_ = parameter.as_int();
+      } else if (param_name == name_ + "." + "combination_method") {
+        combination_method_ = parameter.as_int();
+      }
+    }
+
+  }
+
+  matchSize();
+
+  result.successful = true;
+  return result;
+}
+
 }  // namespace nav2_costmap_2d