From aeae512177c0a3008acf2ef86d4f48ecd2189aba Mon Sep 17 00:00:00 2001 From: Christopher Thompson Date: Wed, 31 Dec 2025 15:40:45 -0600 Subject: [PATCH] Bugfix inactive publishers (#5748) * Do not publish costmap unless active Signed-off-by: Christopher Thompson * Fix style Signed-off-by: Christopher Thompson * Fix typo Signed-off-by: Christopher Thompson * Use layers isEnabled() to prevent publishing Signed-off-by: Christopher Thompson * Fix style Signed-off-by: Christopher Thompson * Add missing include for CostmapLayer type Signed-off-by: Christopher Thompson * Remove extra scoping Signed-off-by: Christopher Thompson --------- Signed-off-by: Christopher Thompson Co-authored-by: Christopher Thompson (cherry picked from commit b766611f334c0de4d0ef3fdf50c9def904effb3a) --- .../nav2_costmap_2d/costmap_2d_publisher.hpp | 13 ++----------- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 7 ++++++- 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp index d6e1b8038b3..891745deebe 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_publisher.hpp +++ b/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_; diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index f7ee5a2c544..8d4b4ee7486 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/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 #include @@ -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) { @@ -238,6 +238,11 @@ std::unique_ptr Costmap2DPublisher::createCostmap void Costmap2DPublisher::publishCostmap() { + auto const costmap_layer = dynamic_cast(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() ||