feat: change publish() function name and improve readability

Signed-off-by: Berkay Karaman <[email protected]>

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/published_time_publisher.hpp

@@ -37,7 +37,7 @@ class PublishedTimePublisher
   {
   }
 
-  void publish(const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp)
+  void publish_if_subscribed(const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp)
   {
     const auto & gid_key = publisher->get_gid();
 
@@ -57,7 +57,7 @@ class PublishedTimePublisher
     }
   }
 
-  void publish(
+  void publish_if_subscribed(
     const rclcpp::PublisherBase::ConstSharedPtr & publisher, const std_msgs::msg::Header & header)
   {
     const auto & gid_key = publisher->get_gid();

control/trajectory_follower_node/src/controller_node.cpp

@@ -233,10 +233,8 @@ void Controller::callbackTimerControl()
   out.longitudinal = lon_out.control_cmd;
   control_cmd_pub_->publish(out);
 
-  // 6. publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(control_cmd_pub_, out.stamp);
-
-  // 7. publish debug marker
+  // 6. publish debug
+  published_time_publisher_->publish_if_subscribed(control_cmd_pub_, out.stamp);
   publishDebugMarker(*input_data, lat_out);
 }
 

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -458,14 +458,8 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
   // Publish commands
   vehicle_cmd_emergency_pub_->publish(vehicle_cmd_emergency);
   control_cmd_pub_->publish(filtered_commands.control);
-
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(control_cmd_pub_, filtered_commands.control.stamp);
-
-  // Publish pause state to api
+  published_time_publisher_->publish_if_subscribed(control_cmd_pub_, filtered_commands.control.stamp);
   adapi_pause_->publish();
-
-  // Publish moderate stop state which is used for stop request
   moderate_stop_interface_->publish();
 
   // Save ControlCmd to steering angle when disengaged

perception/detected_object_feature_remover/README.md

@@ -22,4 +22,6 @@ The `detected_object_feature_remover` is a package to convert topic-type from `D
 
 ## Parameters
 
+None
+
 ## Assumptions / Known limits

perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp

@@ -23,7 +23,6 @@ DetectedObjectFeatureRemover::DetectedObjectFeatureRemover(const rclcpp::NodeOpt
   pub_ = this->create_publisher<DetectedObjects>("~/output", rclcpp::QoS(1));
   sub_ = this->create_subscription<DetectedObjectsWithFeature>(
     "~/input", 1, std::bind(&DetectedObjectFeatureRemover::objectCallback, this, _1));
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -33,9 +32,7 @@ void DetectedObjectFeatureRemover::objectCallback(
   DetectedObjects output;
   convert(*input, output);
   pub_->publish(output);
-
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(pub_, output.header.stamp);
+  published_time_publisher_->publish_if_subscribed(pub_, output.header.stamp);
 }
 
 void DetectedObjectFeatureRemover::convert(

perception/detected_object_validation/src/object_lanelet_filter.cpp

@@ -55,7 +55,6 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
 
   debug_publisher_ =
     std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "object_lanelet_filter");
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -121,9 +120,7 @@ void ObjectLaneletFilterNode::objectCallback(
     ++index;
   }
   object_pub_->publish(output_object_msg);
-
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(object_pub_, output_object_msg.header.stamp);
+  published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp);
 
   // Publish debug info
   const double pipeline_latency =

perception/detected_object_validation/src/object_position_filter.cpp

@@ -42,7 +42,6 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n
     "input/object", rclcpp::QoS{1}, std::bind(&ObjectPositionFilterNode::objectCallback, this, _1));
   object_pub_ = this->create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
     "output/object", rclcpp::QoS{1});
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -67,9 +66,7 @@ void ObjectPositionFilterNode::objectCallback(
   }
 
   object_pub_->publish(output_object_msg);
-
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(object_pub_, output_object_msg.header.stamp);
+  published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp);
 }
 
 bool ObjectPositionFilterNode::isObjectInBounds(

perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

@@ -309,7 +309,6 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
 
   const bool enable_debugger = declare_parameter<bool>("enable_debugger", false);
   if (enable_debugger) debugger_ = std::make_shared<Debugger>(this);
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
@@ -349,10 +348,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
   }
 
   objects_pub_->publish(output);
-
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(objects_pub_, output.header.stamp);
-
+  published_time_publisher_->publish_if_subscribed(objects_pub_, output.header.stamp);
   if (debugger_) {
     debugger_->publishRemovedObjects(removed_objects);
     debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header);

perception/detected_object_validation/src/occupancy_grid_based_validator.cpp

@@ -52,7 +52,6 @@ OccupancyGridBasedValidator::OccupancyGridBasedValidator(const rclcpp::NodeOptio
 
   mean_threshold_ = declare_parameter<float>("mean_threshold", 0.6);
   enable_debug_ = declare_parameter<bool>("enable_debug", false);
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -87,9 +86,7 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid(
   }
 
   objects_pub_->publish(output);
-
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(objects_pub_, output.header.stamp);
+  published_time_publisher_->publish_if_subscribed(objects_pub_, output.header.stamp);
 
   if (enable_debug_) showDebugImage(*input_occ_grid, transformed_objects, occ_grid);
 }

perception/detection_by_tracker/src/detection_by_tracker_core.cpp

@@ -177,7 +177,6 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options)
   cluster_ = std::make_shared<euclidean_cluster::VoxelGridBasedEuclideanCluster>(
     false, 10, 10000, 0.7, 0.3, 0);
   debugger_ = std::make_shared<Debugger>(this);
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -223,10 +222,7 @@ void DetectionByTracker::onObjects(
       !object_recognition_utils::transformObjects(
         objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) {
       objects_pub_->publish(detected_objects);
-
-      // Publish published time only if there are subscribers more than 1
-      published_time_publisher_->publish(objects_pub_, detected_objects.header.stamp);
-
+      published_time_publisher_->publish_if_subscribed(objects_pub_, detected_objects.header.stamp);
       return;
     }
     // to simplify post processes, convert tracked_objects to DetectedObjects message.
@@ -257,10 +253,8 @@ void DetectionByTracker::onObjects(
   }
 
   objects_pub_->publish(detected_objects);
+  published_time_publisher_->publish_if_subscribed(objects_pub_, detected_objects.header.stamp);
   debugger_->publishProcessingTime();
-
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(objects_pub_, detected_objects.header.stamp);
 }
 
 void DetectionByTracker::divideUnderSegmentedObjects(

perception/lidar_centerpoint/src/node.cpp

@@ -126,7 +126,6 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
     RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node.");
     rclcpp::shutdown();
   }
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -165,9 +164,7 @@ void LidarCenterPointNode::pointCloudCallback(
 
   if (objects_sub_count > 0) {
     objects_pub_->publish(output_msg);
-
-    // Publish published time only if there are subscribers more than 1
-    published_time_publisher_->publish(objects_pub_, output_msg.header.stamp);
+    published_time_publisher_->publish_if_subscribed(objects_pub_, output_msg.header.stamp);
   }
 
   // add processing time for debug

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -200,10 +200,6 @@ class MapBasedPredictionNode : public rclcpp::Node
   std::vector<double> distance_set_for_no_intention_to_walk_;
   std::vector<double> timeout_set_for_no_intention_to_walk_;
 
-  // Stop watch
-  StopWatch<std::chrono::milliseconds> stop_watch_;
-
-  // PublishedTime Publisher
   std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 
   // Member Functions

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -812,7 +812,6 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
     std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "map_based_prediction");
 
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
-
   set_param_res_ = this->add_on_set_parameters_callback(
     std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1));
 
@@ -1114,10 +1113,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
 
   // Publish Results
   pub_objects_->publish(output);
-
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(pub_objects_, output.header.stamp);
-
+  published_time_publisher_->publish_if_subscribed(pub_objects_, output.header.stamp);
   pub_debug_markers_->publish(debug_markers);
   const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
   const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -236,7 +236,6 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   data_association_ = std::make_unique<DataAssociation>(
     can_assign_matrix, max_dist_matrix, max_area_matrix, min_area_matrix, max_rad_matrix,
     min_iou_matrix);
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -470,9 +469,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const
 
   // Publish
   tracked_objects_pub_->publish(output_msg);
-
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(tracked_objects_pub_, output_msg.header.stamp);
+  published_time_publisher_->publish_if_subscribed(tracked_objects_pub_, output_msg.header.stamp);
 
   // Debugger Publish if enabled
   debugger_->publishProcessingTime();

perception/object_merger/src/object_association_merger/node.cpp

@@ -125,7 +125,6 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio
   stop_watch_ptr_ = std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
   stop_watch_ptr_->tic("cyclic_time");
   stop_watch_ptr_->tic("processing_time");
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -225,14 +224,12 @@ void ObjectAssociationMergerNode::objectsCallback(
 
   // publish output msg
   merged_object_pub_->publish(output_msg);
+  published_time_publisher_->publish_if_subscribed(merged_object_pub_, output_msg.header.stamp);
   // publish processing time
   processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
     "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true));
   processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
     "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true));
-
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(merged_object_pub_, output_msg.header.stamp);
 }
 }  // namespace object_association
 

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

@@ -234,7 +234,6 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(
   if (enable_debugger) {
     debugger_ptr_ = std::make_shared<Debugger>(*this);
   }
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -330,9 +329,7 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
       return;
     }
     pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr));
-
-    // Publish published time only if there are subscribers more than 1
-    published_time_publisher_->publish(pointcloud_pub_, ogm_frame_filtered_pc.header.stamp);
+    published_time_publisher_->publish_if_subscribed(pointcloud_pub_, ogm_frame_filtered_pc.header.stamp);
   }
   if (debugger_ptr_) {
     debugger_ptr_->publishHighConfidence(high_confidence_pc, ogm_frame_pc.header);

perception/shape_estimation/src/node.cpp

@@ -58,7 +58,6 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option
   stop_watch_ptr_ = std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
   stop_watch_ptr_->tic("cyclic_time");
   stop_watch_ptr_->tic("processing_time");
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -122,8 +121,7 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared
 
   // Publish
   pub_->publish(output_msg);
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(pub_, output_msg.header.stamp);
+  published_time_publisher_->publish_if_subscribed(pub_, output_msg.header.stamp);
   processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
     "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true));
   processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(

perception/tracking_object_merger/src/decorative_tracker_merger.cpp

@@ -158,7 +158,6 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio
   stop_watch_ptr_ = std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
   stop_watch_ptr_->tic("cyclic_time");
   stop_watch_ptr_->tic("processing_time");
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -224,10 +223,7 @@ void DecorativeTrackerMergerNode::mainObjectsCallback(
   this->decorativeMerger(main_sensor_type_, main_objects);
   const auto & tracked_objects = getTrackedObjects(main_objects->header);
   merged_object_pub_->publish(tracked_objects);
-
-  merged_object_pub_->publish(getTrackedObjects(main_objects->header));
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(merged_object_pub_, tracked_objects.header.stamp);
+  published_time_publisher_->publish_if_subscribed(merged_object_pub_, tracked_objects.header.stamp);
   processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
     "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true));
   processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -172,7 +172,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
   }
 
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -415,10 +414,7 @@ void BehaviorPathPlannerNode::run()
 
     if (!path->points.empty()) {
       path_publisher_->publish(*path);
-
-      // Publish published time only if there are subscribers more than 1
-      published_time_publisher_->publish(path_publisher_, path->header.stamp);
-
+      published_time_publisher_->publish_if_subscribed(path_publisher_, path->header.stamp);
     } else {
       RCLCPP_ERROR_THROTTLE(
         get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish.");

planning/behavior_velocity_planner/src/node.cpp

@@ -159,7 +159,6 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
   }
 
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -401,8 +400,7 @@ void BehaviorVelocityPlannerNode::onTrigger(
   lk.unlock();
 
   path_pub_->publish(output_path_msg);
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(path_pub_, output_path_msg.header.stamp);
+  published_time_publisher_->publish_if_subscribed(path_pub_, output_path_msg.header.stamp);
   stop_reason_diag_pub_->publish(planner_manager_.getStopReasonDiag());
 
   if (debug_viz_pub_->get_subscription_count() > 0) {

planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp

@@ -104,7 +104,6 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions
   clock_ = get_clock();
 
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
-
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -316,8 +315,7 @@ void MotionVelocitySmootherNode::publishTrajectory(const TrajectoryPoints & traj
   Trajectory publishing_trajectory = motion_utils::convertToTrajectory(trajectory);
   publishing_trajectory.header = base_traj_raw_ptr_->header;
   pub_trajectory_->publish(publishing_trajectory);
-  // Publish published time only if there are subscribers more than 1
-  published_time_publisher_->publish(pub_trajectory_, publishing_trajectory.header.stamp);
+  published_time_publisher_->publish_if_subscribed(pub_trajectory_, publishing_trajectory.header.stamp);
 }
 
 void MotionVelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg)