publishing only with subscriptions, except for transient local publis…

…hers

Signed-off-by: Steve Macenski <stevenmacenski@gmail.com>

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

nav2_docking/opennav_docking/src/controller.cpp

@@ -121,20 +121,20 @@ bool Controller::isTrajectoryCollisionFree(
   const geometry_msgs::msg::Pose & target_pose, bool is_docking, bool backward)
 {
   // Visualization of the trajectory
-  nav_msgs::msg::Path trajectory;
-  trajectory.header.frame_id = base_frame_;
-  trajectory.header.stamp = clock_->now();
+  auto trajectory = std::make_unique<nav_msgs::msg::Path>();
+  trajectory->header.frame_id = base_frame_;
+  trajectory->header.stamp = clock_->now();
 
   // First pose
   geometry_msgs::msg::PoseStamped next_pose;
   next_pose.header.frame_id = base_frame_;
-  trajectory.poses.push_back(next_pose);
+  trajectory->poses.push_back(next_pose);
 
   // Get the transform from base_frame to fixed_frame
   geometry_msgs::msg::TransformStamped base_to_fixed_transform;
   try {
     base_to_fixed_transform = tf2_buffer_->lookupTransform(
-      fixed_frame_, base_frame_, trajectory.header.stamp,
+      fixed_frame_, base_frame_, trajectory->header.stamp,
       tf2::durationFromSec(transform_tolerance_));
   } catch (tf2::TransformException & ex) {
     RCLCPP_ERROR(
@@ -153,11 +153,11 @@ bool Controller::isTrajectoryCollisionFree(
       simulation_time_step_, target_pose, next_pose.pose, backward);
 
     // Add the pose to the trajectory for visualization
-    trajectory.poses.push_back(next_pose);
+    trajectory->poses.push_back(next_pose);
 
     // Transform pose from base_frame into fixed_frame
     geometry_msgs::msg::PoseStamped local_pose = next_pose;
-    local_pose.header.stamp = trajectory.header.stamp;
+    local_pose.header.stamp = trajectory->header.stamp;
     tf2::doTransform(local_pose, local_pose, base_to_fixed_transform);
 
     // Determine the distance at which to check for collisions
@@ -177,16 +177,19 @@ bool Controller::isTrajectoryCollisionFree(
         logger_, "Collision detected at pose: (%.2f, %.2f, %.2f) in frame %s",
         local_pose.pose.position.x, local_pose.pose.position.y, local_pose.pose.position.z,
         local_pose.header.frame_id.c_str());
-      trajectory_pub_->publish(trajectory);
+      if (trajectory_pub_->get_subscription_count() > 0) {
+        trajectory_pub_->publish(std::move(trajectory));
+      }
       return false;
     }
 
     // Check if we reach the goal
     distance = nav2_util::geometry_utils::euclidean_distance(target_pose, next_pose.pose);
-  }while(distance > 1e-2 && trajectory.poses.size() < max_iter);
-
-  trajectory_pub_->publish(trajectory);
+  }while(distance > 1e-2 && trajectory->poses.size() < max_iter);
 
+  if (trajectory_pub_->get_subscription_count() > 0) {
+    trajectory_pub_->publish(std::move(trajectory));
+  }
   return true;
 }
 

nav2_docking/opennav_docking/src/simple_charging_dock.cpp

@@ -163,15 +163,19 @@ geometry_msgs::msg::PoseStamped SimpleChargingDock::getStagingPose(
   staging_pose.pose.orientation = tf2::toMsg(orientation);
 
   // Publish staging pose for debugging purposes
-  staging_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(staging_pose));
+  if (staging_pose_pub_->get_subscription_count() > 0) {
+    staging_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(staging_pose));
+  }
   return staging_pose;
 }
 
 bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string /*id*/)
 {
   // If using not detection, set the dock pose to the static fixed-frame version
   if (!use_external_detection_pose_) {
-    dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(pose));
+    if (dock_pose_pub_->get_subscription_count() > 0) {
+      dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(pose));
+    }
     dock_pose_ = pose;
     return true;
   }
@@ -207,7 +211,9 @@ bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose,
 
   // Filter the detected pose
   detected = filter_->update(detected);
-  filtered_dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(detected));
+  if (filtered_dock_pose_pub_->get_subscription_count() > 0) {
+    filtered_dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(detected));
+  }
 
   // Rotate the just the orientation, then remove roll/pitch
   geometry_msgs::msg::PoseStamped just_orientation;
@@ -231,7 +237,9 @@ bool SimpleChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose,
   dock_pose_.pose.position.z = 0.0;
 
   // Publish & return dock pose for debugging purposes
-  dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(dock_pose_));
+  if (dock_pose_pub_->get_subscription_count() > 0) {
+    dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(dock_pose_));
+  }
   pose = dock_pose_;
   return true;
 }

nav2_docking/opennav_docking/src/simple_non_charging_dock.cpp

@@ -144,15 +144,19 @@ geometry_msgs::msg::PoseStamped SimpleNonChargingDock::getStagingPose(
   staging_pose.pose.orientation = tf2::toMsg(orientation);
 
   // Publish staging pose for debugging purposes
-  staging_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(staging_pose));
+  if (staging_pose_pub_) {
+    staging_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(staging_pose));
+  }
   return staging_pose;
 }
 
 bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pose, std::string)
 {
   // If using not detection, set the dock pose to the static fixed-frame version
   if (!use_external_detection_pose_) {
-    dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(pose));
+    if (dock_pose_pub_->get_subscription_count() > 0) {
+      dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(pose));
+    }
     dock_pose_ = pose;
     return true;
   }
@@ -188,7 +192,9 @@ bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pos
 
   // Filter the detected pose
   detected = filter_->update(detected);
-  filtered_dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(detected));
+  if (filtered_dock_pose_pub_->get_subscription_count() > 0) {
+    filtered_dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(detected));
+  }
 
   // Rotate the just the orientation, then remove roll/pitch
   geometry_msgs::msg::PoseStamped just_orientation;
@@ -212,7 +218,9 @@ bool SimpleNonChargingDock::getRefinedPose(geometry_msgs::msg::PoseStamped & pos
   dock_pose_.pose.position.z = 0.0;
 
   // Publish & return dock pose for debugging purposes
-  dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(dock_pose_));
+  if (dock_pose_pub_->get_subscription_count() > 0) {
+    dock_pose_pub_->publish(std::make_unique<geometry_msgs::msg::PoseStamped>(dock_pose_));
+  }
   pose = dock_pose_;
   return true;
 }

nav2_graceful_controller/src/graceful_controller.cpp

@@ -140,7 +140,9 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
   validateOrientations(transformed_plan.poses);
 
   // Publish plan for visualization
-  transformed_plan_pub_->publish(transformed_plan);
+  if (transformed_plan_pub_->get_subscription_count() > 0) {
+    transformed_plan_pub_->publish(std::make_unique<nav_msgs::msg::Path>(transformed_plan));
+  }
 
   // Transform local frame to global frame to use in collision checking
   geometry_msgs::msg::TransformStamped costmap_transform;
@@ -233,13 +235,19 @@ geometry_msgs::msg::TwistStamped GracefulController::computeVelocityCommands(
     if (simulateTrajectory(target_pose, costmap_transform, local_plan, cmd_vel, reversing)) {
       // Successfully simulated to target_pose - compute velocity at this moment
       // Publish the selected target_pose
-      motion_target_pub_->publish(nav2_graceful_controller::createMotionTargetMsg(target_pose));
+      if (motion_target_pub_->get_subscription_count() > 0) {
+        motion_target_pub_->publish(nav2_graceful_controller::createMotionTargetMsg(target_pose));
+      }
       // Publish marker for slowdown radius around motion target for debugging / visualization
-      slowdown_pub_->publish(nav2_graceful_controller::createSlowdownMarker(
-        target_pose, params_->slowdown_radius));
+      if (slowdown_pub_->get_subscription_count() > 0) {
+        slowdown_pub_->publish(nav2_graceful_controller::createSlowdownMarker(
+          target_pose, params_->slowdown_radius));
+      }
       // Publish the local plan
       local_plan.header = transformed_plan.header;
-      local_plan_pub_->publish(local_plan);
+      if (local_plan_pub_->get_subscription_count() > 0) {
+        local_plan_pub_->publish(std::make_unique<nav_msgs::msg::Path>(local_plan));
+      }
       // Successfully found velocity command
       return cmd_vel;
     }

nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp

@@ -114,12 +114,16 @@ bool CollisionChecker::isCollisionImminent(
 
     // check for collision at the projected pose
     if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) {
-      carrot_arc_pub_->publish(std::move(arc_pts_msg));
+      if (carrot_arc_pub_->get_subscription_count() > 0) {
+        carrot_arc_pub_->publish(std::move(arc_pts_msg));
+      }
       return true;
     }
   }
 
-  carrot_arc_pub_->publish(std::move(arc_pts_msg));
+  if (carrot_arc_pub_->get_subscription_count() > 0) {
+    carrot_arc_pub_->publish(std::move(arc_pts_msg));
+  }
 
   return false;
 }

nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp

@@ -183,8 +183,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   // Transform path to robot base frame
   auto transformed_plan = path_handler_->transformGlobalPlan(
     pose, params_->max_robot_pose_search_dist, params_->interpolate_curvature_after_goal);
-  auto plan_msg = std::make_unique<nav_msgs::msg::Path>(transformed_plan);
-  global_path_pub_->publish(std::move(plan_msg));
+  if (global_path_pub_->get_subscription_count() > 0) {
+    global_path_pub_->publish(std::make_unique<nav_msgs::msg::Path>(transformed_plan));
+  }
 
   // Find look ahead distance and point on path and publish
   double lookahead_dist = getLookAheadDistance(speed);
@@ -203,7 +204,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   // Get the particular point on the path at the lookahead distance
   auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan);
   auto rotate_to_path_carrot_pose = carrot_pose;
-  carrot_pub_->publish(createCarrotMsg(carrot_pose));
+  if (carrot_pub_->get_subscription_count() > 0) {
+    carrot_pub_->publish(createCarrotMsg(carrot_pose));
+  }
 
   double linear_vel, angular_vel;
 
@@ -216,7 +219,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
       transformed_plan, params_->interpolate_curvature_after_goal);
     rotate_to_path_carrot_pose = curvature_lookahead_pose;
     regulation_curvature = calculateCurvature(curvature_lookahead_pose.pose.position);
-    curvature_carrot_pub_->publish(createCarrotMsg(curvature_lookahead_pose));
+    if (curvature_carrot_pub_->get_subscription_count() > 0) {
+      curvature_carrot_pub_->publish(createCarrotMsg(curvature_lookahead_pose));
+    }
   }
 
   // Setting the velocity direction
@@ -277,9 +282,11 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
   }
 
   // Publish whether we are rotating to goal heading
-  auto is_rotating_to_heading_msg = std::make_unique<std_msgs::msg::Bool>();
-  is_rotating_to_heading_msg->data = is_rotating_to_heading_;
-  is_rotating_to_heading_pub_->publish(std::move(is_rotating_to_heading_msg));
+  if (is_rotating_to_heading_pub_->get_subscription_count() > 0) {
+    auto is_rotating_to_heading_msg = std::make_unique<std_msgs::msg::Bool>();
+    is_rotating_to_heading_msg->data = is_rotating_to_heading_;
+    is_rotating_to_heading_pub_->publish(std::move(is_rotating_to_heading_msg));
+  }
 
   // populate and return message
   geometry_msgs::msg::TwistStamped cmd_vel;