Add ability to publish layers of the costmap

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -319,7 +319,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   // Publishers and subscribers
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
     footprint_pub_;
-  std::unique_ptr<Costmap2DPublisher> costmap_publisher_{nullptr};
+  std::unique_ptr<Costmap2DPublisher> costmap_publisher_;
+
+  std::vector<std::unique_ptr<Costmap2DPublisher>> layer_publishers_;
 
   rclcpp::Subscription<geometry_msgs::msg::Polygon>::SharedPtr footprint_sub_;
   rclcpp::Subscription<rcl_interfaces::msg::ParameterEvent>::SharedPtr parameter_sub_;

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp

@@ -40,6 +40,7 @@
 
 #include <string>
 #include <mutex>
+#include <memory>
 
 #include "geometry_msgs/msg/pose2_d.hpp"
 #include "std_srvs/srv/set_bool.hpp"

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -210,6 +210,19 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
     layered_costmap_->getCostmap(), global_frame_,
     "costmap", always_send_full_costmap_);
 
+  auto layers = layered_costmap_->getPlugins();
+
+  for (auto & layer : *layers) {
+    auto costmap_layer = std::dynamic_pointer_cast<CostmapLayer>(layer);
+    if (costmap_layer != nullptr) {
+      layer_publishers_.emplace_back(
+        std::make_unique<Costmap2DPublisher>(
+          shared_from_this(),
+          costmap_layer.get(), global_frame_,
+          layer->getName(), always_send_full_costmap_));
+    }
+  }
+
   // Set the footprint
   if (use_radius_) {
     setRobotFootprint(makeFootprintFromRadius(robot_radius_));
@@ -233,8 +246,12 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Activating");
 
-  costmap_publisher_->on_activate();
   footprint_pub_->on_activate();
+  costmap_publisher_->on_activate();
+
+  for (auto & layer_pub : layer_publishers_) {
+    layer_pub->on_activate();
+  }
 
   // First, make sure that the transform between the robot base frame
   // and the global frame is available
@@ -280,8 +297,13 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   RCLCPP_INFO(get_logger(), "Deactivating");
 
   dyn_params_handler.reset();
-  costmap_publisher_->on_deactivate();
+
   footprint_pub_->on_deactivate();
+  costmap_publisher_->on_deactivate();
+
+  for (auto & layer_pub : layer_publishers_) {
+    layer_pub->on_deactivate();
+  }
 
   stop();
 
@@ -308,6 +330,10 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   costmap_publisher_.reset();
   clear_costmap_service_.reset();
 
+  for (auto & layer_pub : layer_publishers_) {
+    layer_pub.reset();
+  }
+
   executor_thread_.reset();
   return nav2_util::CallbackReturn::SUCCESS;
 }
@@ -446,12 +472,21 @@ Costmap2DROS::mapUpdateLoop(double frequency)
       layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
       costmap_publisher_->updateBounds(x0, xn, y0, yn);
 
+      for (auto & layer_pub : layer_publishers_) {
+        layer_pub->updateBounds(x0, xn, y0, yn);
+      }
+
       auto current_time = now();
       if ((last_publish_ + publish_cycle_ < current_time) ||  // publish_cycle_ is due
         (current_time < last_publish_))      // time has moved backwards, probably due to a switch to sim_time // NOLINT
       {
         RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str());
         costmap_publisher_->publishCostmap();
+
+        for (auto & layer_pub : layer_publishers_) {
+          layer_pub->publishCostmap();
+        }
+
         last_publish_ = current_time;
       }
     }