feat: add published_time publisher debug to packages

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

control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp

@@ -28,6 +28,7 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
 #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp"
@@ -121,6 +122,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
+
   void publishProcessingTime(
     const double t_ms, const rclcpp::Publisher<Float64Stamped>::SharedPtr pub);
   StopWatch<std::chrono::milliseconds> stop_watch_;

control/trajectory_follower_node/src/controller_node.cpp

@@ -91,6 +91,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
   }
 
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
+
+  published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
 }
 
 Controller::LateralControllerMode Controller::getLateralControllerMode(
@@ -231,7 +233,12 @@ void Controller::callbackTimerControl()
   out.longitudinal = lon_out.control_cmd;
   control_cmd_pub_->publish(out);
 
-  // 6. publish debug marker
+  // 6. publish published time only if enabled by parameter
+  if (published_time_publisher_) {
+    published_time_publisher_->publish(control_cmd_pub_, out.stamp);
+  }
+
+  // 7. publish debug marker
   publishDebugMarker(*input_data, lat_out);
 }
 

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -242,6 +242,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
     this, get_clock(), period_ns, std::bind(&VehicleCmdGate::publishStatus, this));
 
   logger_configure_ = std::make_unique<tier4_autoware_utils::LoggerLevelConfigure>(this);
+
+  published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
 }
 
 bool VehicleCmdGate::isHeartbeatTimeout(
@@ -456,7 +458,16 @@ 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 enabled by parameter
+  if (published_time_publisher_) {
+    published_time_publisher_->publish(control_cmd_pub_, filtered_commands.control.stamp);
+  }
+
+  // Publish pause state to api
   adapi_pause_->publish();
+
+  // Publish moderate stop state which is used for stop request
   moderate_stop_interface_->publish();
 
   // Save ControlCmd to steering angle when disengaged

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -23,6 +23,7 @@
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <motion_utils/vehicle/vehicle_state_checker.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 #include <vehicle_cmd_gate/msg/is_filter_activated.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
@@ -248,6 +249,8 @@ class VehicleCmdGate : public rclcpp::Node
   void publishMarkers(const IsFilterActivated & filter_activated);
 
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
+
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 };
 
 }  // namespace vehicle_cmd_gate

perception/detected_object_feature_remover/include/detected_object_feature_remover/detected_object_feature_remover.hpp

@@ -16,6 +16,7 @@
 #define DETECTED_OBJECT_FEATURE_REMOVER__DETECTED_OBJECT_FEATURE_REMOVER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
@@ -33,6 +34,7 @@ class DetectedObjectFeatureRemover : public rclcpp::Node
 private:
   rclcpp::Subscription<DetectedObjectsWithFeature>::SharedPtr sub_;
   rclcpp::Publisher<DetectedObjects>::SharedPtr pub_;
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
   void objectCallback(const DetectedObjectsWithFeature::ConstSharedPtr input);
   void convert(const DetectedObjectsWithFeature & objs_with_feature, DetectedObjects & objs);
 };

perception/detected_object_feature_remover/package.xml

@@ -13,6 +13,7 @@
   <depend>autoware_auto_perception_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
+  <depend>tier4_autoware_utils</depend>
   <depend>tier4_perception_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

perception/detected_object_feature_remover/src/detected_object_feature_remover.cpp

@@ -23,6 +23,9 @@ 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 enabled only if the use_published_time is true
+  published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
 }
 
 void DetectedObjectFeatureRemover::objectCallback(
@@ -31,6 +34,11 @@ void DetectedObjectFeatureRemover::objectCallback(
   DetectedObjects output;
   convert(*input, output);
   pub_->publish(output);
+
+  // Publish published time if enabled by parameter
+  if (published_time_publisher_) {
+    published_time_publisher_->publish(pub_, output.header.stamp);
+  }
 }
 
 void DetectedObjectFeatureRemover::convert(

perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp

@@ -20,6 +20,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
@@ -68,6 +69,8 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &);
   geometry_msgs::msg::Polygon setFootprint(
     const autoware_auto_perception_msgs::msg::DetectedObject &);
+
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 };
 
 }  // namespace object_lanelet_filter

perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp

@@ -19,6 +19,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
@@ -51,6 +52,8 @@ class ObjectPositionFilterNode : public rclcpp::Node
   float lower_bound_y_;
   utils::FilterTargetLabel filter_target_;
   bool isObjectInBounds(const autoware_auto_perception_msgs::msg::DetectedObject & object) const;
+
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 };
 
 }  // namespace object_position_filter

perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp

@@ -21,6 +21,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
@@ -148,6 +149,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node
   std::shared_ptr<Debugger> debugger_;
   bool using_2d_validator_;
   std::unique_ptr<Validator> validator_;
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 
 private:
   void onObjectsAndObstaclePointCloud(

perception/detected_object_validation/include/detected_object_validation/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp

@@ -19,6 +19,7 @@
 #include <opencv2/imgproc/imgproc.hpp>
 #include <opencv2/opencv.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
@@ -42,6 +43,7 @@ class OccupancyGridBasedValidator : public rclcpp::Node
   message_filters::Subscriber<nav_msgs::msg::OccupancyGrid> occ_grid_sub_;
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 
   typedef message_filters::sync_policies::ApproximateTime<
     autoware_auto_perception_msgs::msg::DetectedObjects, nav_msgs::msg::OccupancyGrid>

perception/detected_object_validation/src/object_lanelet_filter.cpp

@@ -55,6 +55,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
 
   debug_publisher_ =
     std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "object_lanelet_filter");
+
+  // Published Time Publisher enabled only if the use_published_time is true
+  published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
 }
 
 void ObjectLaneletFilterNode::mapCallback(
@@ -120,6 +123,10 @@ void ObjectLaneletFilterNode::objectCallback(
   }
   object_pub_->publish(output_object_msg);
 
+  if (published_time_publisher_) {
+    published_time_publisher_->publish(object_pub_, output_object_msg.header.stamp);
+  }
+
   // Publish debug info
   const double pipeline_latency =
     std::chrono::duration<double, std::milli>(

perception/detected_object_validation/src/object_position_filter.cpp

@@ -42,6 +42,9 @@ 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 enabled only if the use_published_time is true
+  published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
 }
 
 void ObjectPositionFilterNode::objectCallback(
@@ -65,6 +68,10 @@ void ObjectPositionFilterNode::objectCallback(
   }
 
   object_pub_->publish(output_object_msg);
+
+  if (published_time_publisher_) {
+    published_time_publisher_->publish(object_pub_, output_object_msg.header.stamp);
+  }
 }
 
 bool ObjectPositionFilterNode::isObjectInBounds(

perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

@@ -309,6 +309,9 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
 
   const bool enable_debugger = declare_parameter<bool>("enable_debugger", false);
   if (enable_debugger) debugger_ = std::make_shared<Debugger>(this);
+
+  // Published Time Publisher enabled only if the use_published_time is true
+  published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
 }
 void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
@@ -347,6 +350,11 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
   }
 
   objects_pub_->publish(output);
+
+  if (published_time_publisher_) {
+    published_time_publisher_->publish(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,6 +52,9 @@ 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 enabled only if the use_published_time is true
+  published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
 }
 
 void OccupancyGridBasedValidator::onObjectsAndOccGrid(
@@ -86,6 +89,10 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid(
 
   objects_pub_->publish(output);
 
+  if (published_time_publisher_) {
+    published_time_publisher_->publish(objects_pub_, output.header.stamp);
+  }
+
   if (enable_debug_) showDebugImage(*input_occ_grid, transformed_objects, occ_grid);
 }
 

perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp

@@ -23,6 +23,7 @@
 #include <euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <shape_estimation/shape_estimator.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
@@ -83,6 +84,8 @@ class DetectionByTracker : public rclcpp::Node
 
   detection_by_tracker::utils::TrackerIgnoreLabel tracker_ignore_;
 
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
+
   void setMaxSearchRange();
 
   void onObjects(

perception/detection_by_tracker/src/detection_by_tracker_core.cpp

@@ -177,6 +177,9 @@ 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 enabled only if the use_published_time is true
+  published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
 }
 
 void DetectionByTracker::setMaxSearchRange()
@@ -220,6 +223,10 @@ void DetectionByTracker::onObjects(
       !object_recognition_utils::transformObjects(
         objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) {
       objects_pub_->publish(detected_objects);
+
+      if (published_time_publisher_) {
+        published_time_publisher_->publish(objects_pub_, detected_objects.header.stamp);
+      }
       return;
     }
     // to simplify post processes, convert tracked_objects to DetectedObjects message.
@@ -250,6 +257,10 @@ void DetectionByTracker::onObjects(
   }
 
   objects_pub_->publish(detected_objects);
+
+  if (published_time_publisher_) {
+    published_time_publisher_->publish(objects_pub_, detected_objects.header.stamp);
+  }
 }
 
 void DetectionByTracker::divideUnderSegmentedObjects(

perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp

@@ -21,6 +21,7 @@
 #include <lidar_centerpoint/detection_class_remapper.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_object_kinematics.hpp>
@@ -63,6 +64,8 @@ class LidarCenterPointNode : public rclcpp::Node
   std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_{
     nullptr};
   std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_ptr_{nullptr};
+
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 };
 
 }  // namespace centerpoint

perception/lidar_centerpoint/src/node.cpp

@@ -126,6 +126,9 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
     RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node.");
     rclcpp::shutdown();
   }
+
+  // Published Time Publisher enabled only if the use_published_time is true
+  published_time_publisher_ = tier4_autoware_utils::PublishedTimePublisher::create(this);
 }
 
 void LidarCenterPointNode::pointCloudCallback(
@@ -163,6 +166,10 @@ void LidarCenterPointNode::pointCloudCallback(
 
   if (objects_sub_count > 0) {
     objects_pub_->publish(output_msg);
+
+    if (published_time_publisher_) {
+      published_time_publisher_->publish(objects_pub_, output_msg.header.stamp);
+    }
   }
 
   // add processing time for debug

perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp

@@ -21,6 +21,7 @@
 #include "tier4_autoware_utils/ros/update_param.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 #include <tier4_autoware_utils/ros/transform_listener.hpp>
 #include <tier4_autoware_utils/ros/uuid_helper.hpp>
 #include <tier4_autoware_utils/system/stop_watch.hpp>
@@ -199,6 +200,9 @@ class MapBasedPredictionNode : public rclcpp::Node
   // Stop watch
   StopWatch<std::chrono::milliseconds> stop_watch_;
 
+  // PublishedTime Publisher
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
+
   // Member Functions
   void mapCallback(const HADMapBin::ConstSharedPtr msg);
   void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg);