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"
@@ -126,6 +127,12 @@ class CostmapFilter : public Layer
    */
   bool isClearable() {return false;}
 
+  /**
+   * @brief If this layer can be published or not
+   */
+  virtual bool isPublishable() {return false;}
+
+
   /** CostmapFilter API **/
   /**
    * @brief: Initializes costmap filter. Creates subscriptions to filter-related topics

nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp

@@ -133,6 +133,11 @@ class InflationLayer : public Layer
    */
   virtual bool isClearable() {return false;}
 
+  /**
+   * @brief If this layer can be published or not
+   */
+  virtual bool isPublishable() {return false;}
+
   /**
    * @brief Reset this costmap
    */

nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp

@@ -89,6 +89,11 @@ class Layer
    */
   virtual bool isClearable() = 0;
 
+  /**
+   * @brief If publishing operations should be processed on this layer or not
+   */
+  virtual bool isPublishable() = 0;
+
   /**
    * @brief This is called by the LayeredCostmap to poll this plugin as to how
    *        much of the costmap it needs to update. Each layer can increase

nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp

@@ -131,6 +131,11 @@ class ObstacleLayer : public CostmapLayer
    */
   virtual bool isClearable() {return true;}
 
+  /**
+   * @brief If this layer can be published or not
+   */
+  virtual bool isPublishable() {return true;}
+
   /**
    * @brief Callback executed when a parameter change is detected
    * @param event ParameterEvent message

nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp

@@ -123,6 +123,11 @@ class RangeSensorLayer : public CostmapLayer
    */
   virtual bool isClearable() {return true;}
 
+  /**
+   * @brief If this layer can be published or not
+   */
+  virtual bool isPublishable() {return true;}
+
   /**
    * @brief Handle an incoming Range message to populate into costmap
    */

nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp

@@ -92,6 +92,11 @@ class StaticLayer : public CostmapLayer
    */
   virtual bool isClearable() {return false;}
 
+  /**
+   * @brief If this layer can be published or not
+   */
+  virtual bool isPublishable() {return false;}
+
   /**
    * @brief Update the bounds of the master costmap by this layer's update dimensions
    * @param robot_x X pose of robot

nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp

@@ -127,6 +127,11 @@ class VoxelLayer : public ObstacleLayer
    */
   virtual bool isClearable() {return true;}
 
+  /**
+   * @brief If this layer can be published or not
+   */
+  virtual bool isPublishable() {return true;}
+
 protected:
   /**
    * @brief Reset internal maps

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -210,6 +210,20 @@ 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) {
+    if (layer->isPublishable()) {
+      auto costmap_layer = std::static_pointer_cast<CostmapLayer>(layer);
+      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 +247,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 +298,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 +331,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 +473,22 @@ Costmap2DROS::mapUpdateLoop(double frequency)
       layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
       costmap_publisher_->updateBounds(x0, xn, y0, yn);
 
+      // TODO(jwallace42): need bounds of each costmap layer?
+      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;
       }
     }

nav2_costmap_2d/test/unit/declare_parameter_test.cpp

@@ -34,6 +34,7 @@ class LayerWrapper : public nav2_costmap_2d::Layer
   void updateBounds(double, double, double, double *, double *, double *, double *) {}
   void updateCosts(nav2_costmap_2d::Costmap2D &, int, int, int, int) {}
   bool isClearable() {return false;}
+  bool isPublishable() {return false;}
 };
 
 TEST(DeclareParameter, useValidParameter)