TEMP COMMIT - add published time debug class

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

Author: brkay54

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

@@ -0,0 +1,54 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_
+#define TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_common_msgs/msg/published_time.hpp>
+
+namespace tier4_autoware_utils
+{
+using autoware_common_msgs::msg::PublishedTime;
+
+class PublishedTimePublisher
+{
+public:
+  explicit PublishedTimePublisher(
+    rclcpp::Node * node, const std::string & name = "~/debug/published_time",
+    const rclcpp::QoS & qos = rclcpp::QoS(1))
+  {
+    pub_published_time_ = node->create_publisher<PublishedTime>(name, qos);
+  }
+
+  void publish(const rclcpp::Time & header_stamp) const
+  {
+    // Check if there are any subscribers, otherwise don't do anything
+    if (pub_published_time_->get_subscription_count() > 0) {
+      PublishedTime published_time;
+
+      published_time.header_stamp = header_stamp;
+      published_time.published_stamp = rclcpp::Clock().now();
+
+      pub_published_time_->publish(published_time);
+    }
+  }
+
+private:
+  rclcpp::Publisher<PublishedTime>::SharedPtr pub_published_time_;
+};
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_

common/tier4_autoware_utils/package.xml

@@ -15,6 +15,7 @@
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>autoware_common_msgs</depend>
   <depend>builtin_interfaces</depend>
   <depend>diagnostic_msgs</depend>
   <depend>geometry_msgs</depend>

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"
@@ -85,6 +86,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   rclcpp::Publisher<Float64Stamped>::SharedPtr pub_processing_time_lat_ms_;
   rclcpp::Publisher<Float64Stamped>::SharedPtr pub_processing_time_lon_ms_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
+  tier4_autoware_utils::PublishedTimePublisher published_time_publisher_{this};
 
   autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_;
   nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_;

control/trajectory_follower_node/src/controller_node.cpp

@@ -230,6 +230,7 @@ void Controller::callbackTimerControl()
   out.lateral = lat_out.control_cmd;
   out.longitudinal = lon_out.control_cmd;
   control_cmd_pub_->publish(out);
+  published_time_publisher_.publish(out.stamp);
 
   // 6. publish debug marker
   publishDebugMarker(*input_data, lat_out);

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -458,6 +458,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands)
   control_cmd_pub_->publish(filtered_commands.control);
   adapi_pause_->publish();
   moderate_stop_interface_->publish();
+  published_time_publisher_.publish(filtered_commands.control.stamp);
 
   // Save ControlCmd to steering angle when disengaged
   prev_control_cmd_ = filtered_commands.control;

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>
 
@@ -110,6 +111,7 @@ class VehicleCmdGate : public rclcpp::Node
   rclcpp::Publisher<MarkerArray>::SharedPtr filter_activated_marker_pub_;
   rclcpp::Publisher<MarkerArray>::SharedPtr filter_activated_marker_raw_pub_;
   rclcpp::Publisher<BoolStamped>::SharedPtr filter_activated_flag_pub_;
+  tier4_autoware_utils::PublishedTimePublisher published_time_publisher_{this};
 
   // Subscription
   rclcpp::Subscription<Heartbeat>::SharedPtr external_emergency_stop_heartbeat_sub_;

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_;
+  tier4_autoware_utils::PublishedTimePublisher published_time_publisher_{this};
   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

@@ -31,6 +31,7 @@ void DetectedObjectFeatureRemover::objectCallback(
   DetectedObjects output;
   convert(*input, output);
   pub_->publish(output);
+  published_time_publisher_.publish(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>
@@ -51,6 +52,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr map_sub_;
   rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_sub_;
   std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_{nullptr};
+  tier4_autoware_utils::PublishedTimePublisher published_time_publisher_{this};
 
   lanelet::LaneletMapPtr lanelet_map_ptr_;
   lanelet::ConstLanelets road_lanelets_;

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>
@@ -41,6 +42,7 @@ class ObjectPositionFilterNode : public rclcpp::Node
 
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_pub_;
   rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr object_sub_;
+  tier4_autoware_utils::PublishedTimePublisher published_time_publisher_{this};
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;

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>
@@ -144,6 +145,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node
   typedef message_filters::Synchronizer<SyncPolicy> Sync;
   Sync sync_;
   PointsNumThresholdParam points_num_threshold_param_;
+  tier4_autoware_utils::PublishedTimePublisher published_time_publisher_{this};
 
   std::shared_ptr<Debugger> debugger_;
   bool using_2d_validator_;

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_;
+  tier4_autoware_utils::PublishedTimePublisher published_time_publisher_{this};
 
   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

@@ -119,6 +119,7 @@ void ObjectLaneletFilterNode::objectCallback(
     ++index;
   }
   object_pub_->publish(output_object_msg);
+  published_time_publisher_.publish(output_object_msg.header.stamp);
 
   // Publish debug info
   const double pipeline_latency =

perception/detected_object_validation/src/object_position_filter.cpp

@@ -65,6 +65,7 @@ void ObjectPositionFilterNode::objectCallback(
   }
 
   object_pub_->publish(output_object_msg);
+  published_time_publisher_.publish(output_object_msg.header.stamp);
 }
 
 bool ObjectPositionFilterNode::isObjectInBounds(

perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

@@ -347,6 +347,8 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
   }
 
   objects_pub_->publish(output);
+  published_time_publisher_.publish(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

@@ -85,6 +85,7 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid(
   }
 
   objects_pub_->publish(output);
+  published_time_publisher_.publish(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>
@@ -70,6 +71,7 @@ class DetectionByTracker : public rclcpp::Node
   rclcpp::Subscription<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr trackers_sub_;
   rclcpp::Subscription<tier4_perception_msgs::msg::DetectedObjectsWithFeature>::SharedPtr
     initial_objects_sub_;
+  tier4_autoware_utils::PublishedTimePublisher published_time_publisher_{this};
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;

perception/detection_by_tracker/src/detection_by_tracker_core.cpp

@@ -220,6 +220,7 @@ void DetectionByTracker::onObjects(
       !object_recognition_utils::transformObjects(
         objects, input_msg->header.frame_id, tf_buffer_, transformed_objects)) {
       objects_pub_->publish(detected_objects);
+      published_time_publisher_.publish(detected_objects.header.stamp);
       return;
     }
     // to simplify post processes, convert tracked_objects to DetectedObjects message.
@@ -250,6 +251,7 @@ void DetectionByTracker::onObjects(
   }
 
   objects_pub_->publish(detected_objects);
+  published_time_publisher_.publish(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,7 @@ 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};
+  tier4_autoware_utils::PublishedTimePublisher published_time_publisher_{this};
 };
 
 }  // namespace centerpoint

perception/lidar_centerpoint/src/node.cpp

@@ -163,6 +163,7 @@ void LidarCenterPointNode::pointCloudCallback(
 
   if (objects_sub_count > 0) {
     objects_pub_->publish(output_msg);
+    published_time_publisher_.publish(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>
@@ -129,6 +130,7 @@ class MapBasedPredictionNode : public rclcpp::Node
   rclcpp::Subscription<TrackedObjects>::SharedPtr sub_objects_;
   rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
   rclcpp::Subscription<TrafficSignalArray>::SharedPtr sub_traffic_signals_;
+  tier4_autoware_utils::PublishedTimePublisher published_time_publisher_{this};
 
   // Object History
   std::unordered_map<std::string, std::deque<ObjectData>> objects_history_;

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -1115,6 +1115,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
 
   // Publish Results
   pub_objects_->publish(output);
+  published_time_publisher_.publish(output.header.stamp);
   pub_debug_markers_->publish(debug_markers);
   const auto calculation_time_msg = createStringStamped(now(), stop_watch_.toc());
   pub_calculation_time_->publish(calculation_time_msg);

perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp

@@ -24,6 +24,7 @@
 
 #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_objects.hpp>
@@ -98,6 +99,7 @@ class MultiObjectTracker : public rclcpp::Node
   rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
     detected_object_sub_;
   rclcpp::TimerBase::SharedPtr publish_timer_;  // publish timer
+  tier4_autoware_utils::PublishedTimePublisher published_time_publisher_{this};
 
   // debugger class
   std::unique_ptr<TrackerDebugger> debugger_;

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -468,6 +468,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time)
 
   // Publish
   tracked_objects_pub_->publish(output_msg);
+  published_time_publisher_.publish(output_msg.header.stamp);
 
   // Debugger Publish if enabled
   debugger_->publishProcessingTime();

perception/object_merger/include/object_merger/node.hpp

@@ -18,6 +18,7 @@
 #include "object_merger/data_association/data_association.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 
@@ -71,6 +72,7 @@ class ObjectAssociationMergerNode : public rclcpp::Node
   int sync_queue_size_;
   std::unique_ptr<DataAssociation> data_association_;
   std::string base_link_frame_id_;  // associated with the base_link frame
+  tier4_autoware_utils::PublishedTimePublisher published_time_publisher_{this};
 
   PriorityMode priority_mode_;
   bool remove_overlapped_unknown_objects_;

perception/object_merger/src/object_association_merger/node.cpp

@@ -215,6 +215,7 @@ void ObjectAssociationMergerNode::objectsCallback(
 
   // publish output msg
   merged_object_pub_->publish(output_msg);
+  published_time_publisher_.publish(output_msg.header.stamp);
 }
 }  // namespace object_association
 

perception/occupancy_grid_map_outlier_filter/include/occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp

@@ -19,6 +19,7 @@
 
 #include <pcl/common/impl/common.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/published_time_publisher.hpp>
 
 #include <geometry_msgs/msg/pose.hpp>
 #include <nav_msgs/msg/occupancy_grid.hpp>
@@ -120,6 +121,7 @@ class OccupancyGridMapOutlierFilterComponent : public rclcpp::Node
   std::shared_ptr<Debugger> debugger_ptr_;
   std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
   std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_;
+  tier4_autoware_utils::PublishedTimePublisher published_time_publisher_{this};
 
   // ROS Parameters
   std::string map_frame_;

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

@@ -305,6 +305,7 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2(
       return;
     }
     pointcloud_pub_->publish(std::move(base_link_frame_filtered_pc_ptr));
+    published_time_publisher_.publish(ogm_frame_pc.header.stamp);
   }
   if (debugger_ptr_) {
     debugger_ptr_->publishHighConfidence(high_confidence_pc, ogm_frame_pc.header);