Bugfix inactive publishers (ros-navigation#5748)

* Do not publish costmap unless active

Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>

* Fix style

Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>

* Fix typo

Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>

* Use layers isEnabled() to prevent publishing

Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>

* Fix style

Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>

* Add missing include for CostmapLayer type

Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>

* Remove extra scoping

Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>

---------

Signed-off-by: Christopher Thompson <cthompson@metalsharkboats.com>
Co-authored-by: Christopher Thompson <cthompson@metalsharkboats.com>

Author: 2 people

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp

@@ -123,19 +123,11 @@ class Costmap2DPublisher
   }
 
   /**
-   * @brief  Publishes the visualization data over ROS
+   * @brief Publishes the visualization data over ROS
+   * @note Only publishes when the associated layer is enabled
    */
   void publishCostmap();
 
-  /**
-   * @brief Check if the publisher is active
-   * @return True if the frequency for the publisher is non-zero, false otherwise
-   */
-  bool active()
-  {
-    return active_;
-  }
-
 private:
   /** @brief Prepare grid_ message for publication. */
   void prepareGrid();
@@ -166,7 +158,6 @@ class Costmap2DPublisher
   unsigned int x0_, xn_, y0_, yn_;
   double saved_origin_x_;
   double saved_origin_y_;
-  bool active_;
   bool always_send_full_costmap_;
   double map_vis_z_;
 

nav2_costmap_2d/src/costmap_2d_publisher.cpp

@@ -37,6 +37,7 @@
  *         David V. Lu!!
  *********************************************************************/
 #include "nav2_costmap_2d/costmap_2d_publisher.hpp"
+#include "nav2_costmap_2d/costmap_layer.hpp"
 
 #include <string>
 #include <memory>
@@ -59,7 +60,6 @@ Costmap2DPublisher::Costmap2DPublisher(
 : costmap_(costmap),
   global_frame_(global_frame),
   topic_name_(topic_name),
-  active_(false),
   always_send_full_costmap_(always_send_full_costmap),
   map_vis_z_(map_vis_z)
 {
@@ -236,6 +236,11 @@ std::unique_ptr<nav2_msgs::msg::CostmapUpdate> Costmap2DPublisher::createCostmap
 
 void Costmap2DPublisher::publishCostmap()
 {
+  auto const costmap_layer = dynamic_cast<CostmapLayer *>(costmap_);
+  if (costmap_layer != nullptr && !costmap_layer->isEnabled()) {
+    return;
+  }
+
   float resolution = costmap_->getResolution();
   if (always_send_full_costmap_ || grid_resolution_ != resolution ||
     grid_width_ != costmap_->getSizeInCellsX() ||