publishing only with subscriptions, except for transient local publishers

Signed-off-by: Steve Macenski <[email protected]>

Author: SteveMacenski

nav2_amcl/src/amcl_node.cpp

@@ -873,7 +873,7 @@ void
 AmclNode::publishParticleCloud(const pf_sample_set_t * set)
 {
   // If initial pose is not known, AMCL does not know the current pose
-  if (!initial_pose_is_known_) {return;}
+  if (!initial_pose_is_known_ || particle_cloud_pub_->get_subscription_count() < 1) {return;}
   auto cloud_with_weights_msg = std::make_unique<nav2_msgs::msg::ParticleCloud>();
   cloud_with_weights_msg->header.stamp = this->now();
   cloud_with_weights_msg->header.frame_id = global_frame_id_;

nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp

@@ -89,10 +89,12 @@ class RosTopicLogger : public BT::StatusChangeLogger
   void flush() override
   {
     if (!event_log_.empty()) {
-      auto log_msg = std::make_unique<nav2_msgs::msg::BehaviorTreeLog>();
-      log_msg->timestamp = clock_->now();
-      log_msg->event_log = event_log_;
-      log_pub_->publish(std::move(log_msg));
+      if (log_pub_->get_subscription_count() > 0) {
+        auto log_msg = std::make_unique<nav2_msgs::msg::BehaviorTreeLog>();
+        log_msg->timestamp = clock_->now();
+        log_msg->event_log = event_log_;
+        log_pub_->publish(std::move(log_msg));
+      }
       event_log_.clear();
     }
   }

nav2_collision_monitor/src/collision_detector_node.cpp

@@ -376,7 +376,9 @@ void CollisionDetector::process()
         collision_points) >= polygon->getMinPoints());
   }
 
-  state_pub_->publish(std::move(state_msg));
+  if (state_pub_->get_subscription_count() > 0) {
+    state_pub_->publish(std::move(state_msg));
+  }
 
   // Publish polygons for better visualization
   publishPolygons();

nav2_collision_monitor/src/polygon.cpp

@@ -310,8 +310,9 @@ void Polygon::publish()
 
   // Actualize the time to current and publish the polygon
   polygon_.header.stamp = node->now();
-  auto msg = std::make_unique<geometry_msgs::msg::PolygonStamped>(polygon_);
-  polygon_pub_->publish(std::move(msg));
+  if (polygon_pub_->get_subscription_count() > 0) {
+    polygon_pub_->publish(std::make_unique<geometry_msgs::msg::PolygonStamped>(polygon_));
+  }
 }
 
 bool Polygon::getCommonParameters(

nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp

@@ -255,10 +255,12 @@ void BinaryFilter::changeState(const bool state)
   }
 
   // Forming and publishing new BinaryState message
-  std::unique_ptr<std_msgs::msg::Bool> msg =
-    std::make_unique<std_msgs::msg::Bool>();
-  msg->data = state;
-  binary_state_pub_->publish(std::move(msg));
+  if (binary_state_pub_->get_subscription_count() > 0) {
+    std::unique_ptr<std_msgs::msg::Bool> msg =
+      std::make_unique<std_msgs::msg::Bool>();
+    msg->data = state;
+    binary_state_pub_->publish(std::move(msg));
+  }
 }
 
 }  // namespace nav2_costmap_2d

nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp

@@ -247,13 +247,15 @@ void SpeedFilter::process(
     }
 
     // Forming and publishing new SpeedLimit message
-    std::unique_ptr<nav2_msgs::msg::SpeedLimit> msg =
-      std::make_unique<nav2_msgs::msg::SpeedLimit>();
-    msg->header.frame_id = global_frame_;
-    msg->header.stamp = clock_->now();
-    msg->percentage = percentage_;
-    msg->speed_limit = speed_limit_;
-    speed_limit_pub_->publish(std::move(msg));
+    if (speed_limit_pub_->get_subscription_count() > 0) {
+      std::unique_ptr<nav2_msgs::msg::SpeedLimit> msg =
+        std::make_unique<nav2_msgs::msg::SpeedLimit>();
+      msg->header.frame_id = global_frame_;
+      msg->header.stamp = clock_->now();
+      msg->percentage = percentage_;
+      msg->speed_limit = speed_limit_;
+      speed_limit_pub_->publish(std::move(msg));
+    }
 
     speed_limit_prev_ = speed_limit_;
   }

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -237,7 +237,7 @@ void VoxelLayer::updateBounds(
     }
   }
 
-  if (publish_voxel_) {
+  if (publish_voxel_ && voxel_pub_->get_subscription_count() > 0) {
     auto grid_msg = std::make_unique<nav2_msgs::msg::VoxelGrid>();
     unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
     grid_msg->size_x = voxel_grid_.sizeX();
@@ -406,7 +406,7 @@ void VoxelLayer::raytraceFreespace(
     }
   }
 
-  if (publish_clearing_points) {
+  if (publish_clearing_points && clearing_endpoints_pub_->get_subscription_count() > 0) {
     clearing_endpoints_->header.frame_id = global_frame_;
     clearing_endpoints_->header.stamp = clearing_observation.cloud_->header.stamp;
 

nav2_costmap_2d/src/costmap_2d_cloud.cpp

@@ -193,12 +193,14 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
   pcl_header.frame_id = frame_id;
   pcl_header.stamp = stamp;
 
+  if (pub_marked->get_subscription_count() > 0)
   {
     auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
     pointCloud2Helper(cloud, num_marked, pcl_header, g_marked);
     pub_marked->publish(std::move(cloud));
   }
 
+  if (pub_unknown->get_subscription_count() > 0)
   {
     auto cloud = std::make_unique<sensor_msgs::msg::PointCloud2>();
     pointCloud2Helper(cloud, num_unknown, pcl_header, g_unknown);

nav2_costmap_2d/src/costmap_2d_markers.cpp

@@ -138,7 +138,9 @@ void voxelCallback(const nav2_msgs::msg::VoxelGrid::ConstSharedPtr grid)
     p.z = c.z;
   }
 
-  pub->publish(std::move(m));
+  if (pub->get_subscription_count() > 0) {
+    pub->publish(std::move(m));
+  }
 
   timer.end();
   RCLCPP_INFO(

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -583,7 +583,9 @@ Costmap2DROS::updateMap()
       transformFootprint(x, y, yaw, padded_footprint_, *footprint);
 
       RCLCPP_DEBUG(get_logger(), "Publishing footprint");
-      footprint_pub_->publish(std::move(footprint));
+      if (footprint_pub_->get_subscription_count() > 0) {
+        footprint_pub_->publish(std::move(footprint));
+      }
       initialized_ = true;
     }
   }