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 a6247e7d120..f0f7d1bd04d 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 a811648fe29..45ae3a534db 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) { @@ -236,6 +236,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() ||