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;
  }
  }