Bringing back dynamic parameters: Costmap2D

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -50,6 +50,7 @@
 #include "nav2_costmap_2d/layered_costmap.hpp"
 #include "nav2_costmap_2d/layer.hpp"
 #include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/parameter_events_subscriber.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "tf2/convert.h"
 #include "tf2/LinearMath/Transform.h"
@@ -279,6 +280,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode
 
   // Parameters
   void getParameters();
+  void paramEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr & event);
+
   bool always_send_full_costmap_{false};
   std::string footprint_;
   float footprint_padding_{0};
@@ -304,6 +307,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   std::vector<geometry_msgs::msg::Point> padded_footprint_;
 
   std::unique_ptr<ClearCostmapService> clear_costmap_service_;
+
+  std::shared_ptr<nav2_util::ParameterEventsSubscriber> param_subscriber_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -169,6 +169,8 @@ class InflationLayer : public Layer
     unsigned int index, unsigned int mx, unsigned int my,
     unsigned int src_x, unsigned int src_y);
 
+  void setParamCallbacks();
+
   double inflation_radius_, inscribed_radius_, cost_scaling_factor_;
   bool inflate_unknown_;
   unsigned int cell_inflation_radius_;
@@ -185,6 +187,7 @@ class InflationLayer : public Layer
 
   // Indicates that the entire costmap should be reinflated next time around.
   bool need_reinflation_;
+  std::recursive_mutex mutex_;
 };
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

@@ -46,6 +46,7 @@
 #include "nav2_costmap_2d/costmap_2d.hpp"
 #include "nav2_costmap_2d/layered_costmap.hpp"
 #include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/parameter_events_subscriber.hpp"
 
 namespace nav2_costmap_2d
 {
@@ -64,7 +65,8 @@ class Layer  // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface
     tf2_ros::Buffer * tf,
     nav2_util::LifecycleNode::SharedPtr node,
     rclcpp::Node::SharedPtr client_node,
-    rclcpp::Node::SharedPtr rclcpp_node);
+    rclcpp::Node::SharedPtr rclcpp_node,
+    nav2_util::ParameterEventsSubscriber * param_subscriber = nullptr);
   virtual void deactivate() {} /** @brief Stop publishers. */
   virtual void activate() {}   /** @brief Restart publishers if they've been stopped. */
   virtual void reset() {}
@@ -135,6 +137,8 @@ class Layer  // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface
   nav2_util::LifecycleNode::SharedPtr node_;
   rclcpp::Node::SharedPtr client_node_;
   rclcpp::Node::SharedPtr rclcpp_node_;
+  nav2_util::ParameterEventsSubscriber * param_subscriber_;
+  std::vector<nav2_util::ParameterEventsCallbackHandle::SharedPtr> callback_handles_;
 
   /** @brief This is called at the end of initialize().  Override to
    * implement subclass-specific initialization.

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

@@ -160,6 +160,8 @@ class ObstacleLayer : public CostmapLayer
     double * max_x,
     double * max_y);
 
+  void setParamCallbacks();
+
   std::string global_frame_;  ///< @brief The global frame for the costmap
   double max_obstacle_height_;  ///< @brief Max Obstacle Height
 

nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp

@@ -85,7 +85,7 @@ class VoxelLayer : public ObstacleLayer
   virtual void resetMaps();
 
 private:
-  void reconfigureCB();
+  void setParamCallbacks();
   void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info);
   virtual void raytraceFreespace(
     const nav2_costmap_2d::Observation & clearing_observation,

nav2_costmap_2d/plugins/inflation_layer.cpp

@@ -75,6 +75,8 @@ InflationLayer::InflationLayer()
 void
 InflationLayer::onInitialize()
 {
+  RCLCPP_INFO(node_->get_logger(), "Initializing inflation layer!");
+
   declareParameter("enabled", rclcpp::ParameterValue(true));
   declareParameter("inflation_radius", rclcpp::ParameterValue(0.55));
   declareParameter("cost_scaling_factor", rclcpp::ParameterValue(10.0));
@@ -90,6 +92,42 @@ InflationLayer::onInitialize()
   need_reinflation_ = false;
   cell_inflation_radius_ = cellDistance(inflation_radius_);
   matchSize();
+  setParamCallbacks();
+}
+
+void
+InflationLayer::setParamCallbacks()
+{
+  if (param_subscriber_) {
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled",
+      [&](const rclcpp::Parameter & p) {
+        enabled_ = p.get_value<bool>();
+        need_reinflation_ = true;
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
+      ".inflate_unknown",
+      [&](const rclcpp::Parameter & p) {
+        inflate_unknown_ = p.get_value<bool>();
+        need_reinflation_ = true;
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
+      ".cost_scaling_factor",
+      [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
+        cost_scaling_factor_ = p.get_value<double>();
+        need_reinflation_ = true;
+        computeCaches();
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
+      ".inflation_radius",
+      [&](const rclcpp::Parameter & p) {
+        std::lock_guard<std::recursive_mutex> lock(mutex_);
+        inflation_radius_ = p.get_value<double>();
+        cell_inflation_radius_ = cellDistance(inflation_radius_);
+        need_reinflation_ = true;
+        computeCaches();
+      }));
+  }
 }
 
 void

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -243,6 +243,33 @@ void ObstacleLayer::onInitialize()
       observation_notifiers_.back()->setTargetFrames(target_frames);
     }
   }
+  setParamCallbacks();
+}
+
+void
+ObstacleLayer::setParamCallbacks()
+{
+  if (param_subscriber_) {
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled",
+      [&](const rclcpp::Parameter & p) {
+        enabled_ = p.get_value<bool>();
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
+      ".footprint_clearing_enabled",
+      [&](const rclcpp::Parameter & p) {
+        footprint_clearing_enabled_ = p.get_value<bool>();
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
+      ".max_obstacle_height",
+      [&](const rclcpp::Parameter & p) {
+        max_obstacle_height_ = p.get_value<double>();
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
+      ".combination_method",
+      [&](const rclcpp::Parameter & p) {
+        combination_method_ = p.get_value<int>();
+      }));
+  }
 }
 
 void

nav2_costmap_2d/plugins/static_layer.cpp

@@ -94,6 +94,19 @@ StaticLayer::onInitialize()
       rclcpp::SystemDefaultsQoS(),
       std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1));
   }
+
+  auto cb = [&](const rclcpp::Parameter & p) {
+      if (enabled_ != p.get_value<bool>()) {
+        enabled_ = p.get_value<bool>();
+        has_updated_data_ = true;
+        x_ = y_ = 0;
+        width_ = size_x_;
+        height_ = size_y_;
+      }
+    };
+  if (param_subscriber_) {
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled", cb));
+  }
 }
 
 void

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -95,6 +95,54 @@ void VoxelLayer::onInitialize()
 
   unknown_threshold_ += (VOXEL_BITS - size_z_);
   matchSize();
+  setParamCallbacks();
+}
+
+void
+VoxelLayer::setParamCallbacks()
+{
+  if (param_subscriber_) {
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".enabled",
+      [&](const rclcpp::Parameter & p) {
+        enabled_ = p.get_value<bool>();
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
+      ".footprint_clearing_enabled",
+      [&](const rclcpp::Parameter & p) {
+        footprint_clearing_enabled_ = p.get_value<bool>();
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
+      ".max_obstacle_height",
+      [&](const rclcpp::Parameter & p) {
+        max_obstacle_height_ = p.get_value<double>();
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
+      ".combination_method",
+      [&](const rclcpp::Parameter & p) {
+        combination_method_ = p.get_value<int>();
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_voxels",
+      [&](const rclcpp::Parameter & p) {
+        size_z_ = p.get_value<int>();
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".origin_z",
+      [&](const rclcpp::Parameter & p) {
+        origin_z_ = p.get_value<double>();
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".z_resolution",
+      [&](const rclcpp::Parameter & p) {
+        z_resolution_ = p.get_value<double>();
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ +
+      ".unknown_threshold",
+      [&](const rclcpp::Parameter & p) {
+        unknown_threshold_ = p.get_value<int>() + (VOXEL_BITS - size_z_);
+      }));
+    callback_handles_.push_back(param_subscriber_->add_parameter_callback(name_ + ".mark_threshold",
+      [&](const rclcpp::Parameter & p) {
+        mark_threshold_ = p.get_value<int>();
+      }));
+  }
 }
 
 VoxelLayer::~VoxelLayer()

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -137,6 +137,8 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
   tf_buffer_->setCreateTimerInterface(timer_interface);
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
+  param_subscriber_ = std::make_shared<nav2_util::ParameterEventsSubscriber>(shared_from_this());
+
   // Then load and add the plug-ins to the costmap
   for (unsigned int i = 0; i < plugin_names_.size(); ++i) {
     RCLCPP_INFO(get_logger(), "Using plugin \"%s\"", plugin_names_[i].c_str());
@@ -146,7 +148,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
     // TODO(mjeronimo): instead of get(), use a shared ptr
     plugin->initialize(layered_costmap_, plugin_names_[i], tf_buffer_.get(),
-      shared_from_this(), client_node_, rclcpp_node_);
+      shared_from_this(), client_node_, rclcpp_node_, param_subscriber_.get());
 
     RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str());
   }
@@ -175,6 +177,8 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
   // Add cleaning service
   clear_costmap_service_ = std::make_unique<ClearCostmapService>(shared_from_this(), *this);
 
+  param_subscriber_->set_event_callback(
+    std::bind(&Costmap2DROS::paramEventCallback, this, std::placeholders::_1));
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -279,6 +283,72 @@ Costmap2DROS::on_shutdown(const rclcpp_lifecycle::State &)
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
+void
+Costmap2DROS::paramEventCallback(const rcl_interfaces::msg::ParameterEvent::SharedPtr & /*event*/)
+{
+  if (map_update_thread_ != NULL) {
+    map_update_thread_shutdown_ = true;
+    map_update_thread_->join();
+    delete map_update_thread_;
+  }
+  map_update_thread_shutdown_ = false;
+
+  get_parameter("transform_tolerance", transform_tolerance_);
+  get_parameter("map_update_frequency", map_update_frequency_);
+  get_parameter("map_publish_frequency", map_publish_frequency_);
+  if (map_publish_frequency_ > 0) {
+    publish_cycle_ = rclcpp::Duration::from_seconds(1 / map_publish_frequency_);
+  } else {
+    publish_cycle_ = rclcpp::Duration(-1);
+  }
+
+  get_parameter("width", map_width_meters_);
+  get_parameter("height", map_height_meters_);
+  get_parameter("resolution", resolution_);
+  get_parameter("origin_x", origin_x_);
+  get_parameter("origin_y", origin_y_);
+  if (!layered_costmap_->isSizeLocked()) {
+    layered_costmap_->resizeMap((unsigned int)(map_width_meters_ / resolution_),
+      (unsigned int)(map_height_meters_ / resolution_), resolution_, origin_x_, origin_y_);
+  }
+
+  double footprint_padding;
+  get_parameter("footprint_padding", footprint_padding);
+  if (footprint_padding_ != footprint_padding) {
+    footprint_padding_ = footprint_padding;
+    setRobotFootprint(unpadded_footprint_);
+  }
+
+  std::string footprint;
+  double robot_radius;
+  get_parameter("footprint", footprint);
+  get_parameter("robot_radius", robot_radius);
+  if (footprint_ != footprint || robot_radius_ != robot_radius) {
+    footprint_ = footprint;
+    robot_radius_ = robot_radius;
+    use_radius_ = true;
+    if (footprint_ != "" && footprint_ != "[]") {
+      std::vector<geometry_msgs::msg::Point> new_footprint;
+      if (makeFootprintFromString(footprint_, new_footprint)) {
+        use_radius_ = false;
+      } else {
+        RCLCPP_ERROR(
+          get_logger(), "The footprint parameter is invalid: \"%s\", using radius (%lf) instead",
+          footprint_.c_str(), robot_radius_);
+      }
+    }
+    if (use_radius_) {
+      setRobotFootprint(makeFootprintFromRadius(robot_radius_));
+    } else {
+      std::vector<geometry_msgs::msg::Point> new_footprint;
+      makeFootprintFromString(footprint_, new_footprint);
+      setRobotFootprint(new_footprint);
+    }
+  }
+  map_update_thread_ = new std::thread(std::bind(
+        &Costmap2DROS::mapUpdateLoop, this, map_update_frequency_));
+}
+
 void
 Costmap2DROS::getParameters()
 {

nav2_costmap_2d/src/layer.cpp

@@ -49,14 +49,16 @@ Layer::initialize(
   LayeredCostmap * parent, std::string name, tf2_ros::Buffer * tf,
   nav2_util::LifecycleNode::SharedPtr node,
   rclcpp::Node::SharedPtr client_node,
-  rclcpp::Node::SharedPtr rclcpp_node)
+  rclcpp::Node::SharedPtr rclcpp_node,
+  nav2_util::ParameterEventsSubscriber * param_subscriber)
 {
   layered_costmap_ = parent;
   name_ = name;
   tf_ = tf;
   node_ = node;
   client_node_ = client_node;
   rclcpp_node_ = rclcpp_node;
+  param_subscriber_ = param_subscriber;
 
   onInitialize();
 }