feat(map_based_prediction, etc): publish processing time in various perception nodes (autowarefoundation#6574)

* feat: fix processing time publisher

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* feat: add processing_time in detection_by_tracker

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* feat: add processing time publisher to shape estimator

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* feat: add latency time for association merger

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* feat: add processing time publisher for tracker merger

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* chore: rename pipeline latency in multi object tracker

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

---------

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
Signed-off-by: kaigohirao <kaigo.hirao@proxima-ai-tech.com>

Author: YoshiRi

perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp

@@ -20,6 +20,8 @@
 #include <euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <shape_estimation/shape_estimator.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
@@ -61,6 +63,11 @@ class Debugger
     divided_objects_pub_ =
       node->create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
         "debug/divided_objects", 1);
+    processing_time_publisher_ =
+      std::make_unique<tier4_autoware_utils::DebugPublisher>(node, "detection_by_tracker");
+    stop_watch_ptr_ =
+      std::make_unique<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>>();
+    this->startStopWatch();
   }
 
   ~Debugger() {}
@@ -80,6 +87,19 @@ class Debugger
   {
     divided_objects_pub_->publish(removeFeature(input));
   }
+  void startStopWatch()
+  {
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
+  void startMeasureProcessingTime() { stop_watch_ptr_->tic("processing_time"); }
+  void publishProcessingTime()
+  {
+    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));
+  }
 
 private:
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
@@ -90,6 +110,9 @@ class Debugger
     merged_objects_pub_;
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
     divided_objects_pub_;
+  // debug publisher
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
 
   autoware_auto_perception_msgs::msg::DetectedObjects removeFeature(
     const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input)

perception/detection_by_tracker/src/detection_by_tracker_core.cpp

@@ -206,6 +206,7 @@ void DetectionByTracker::setMaxSearchRange()
 void DetectionByTracker::onObjects(
   const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg)
 {
+  debugger_->startMeasureProcessingTime();
   autoware_auto_perception_msgs::msg::DetectedObjects detected_objects;
   detected_objects.header = input_msg->header;
 
@@ -250,6 +251,7 @@ void DetectionByTracker::onObjects(
   }
 
   objects_pub_->publish(detected_objects);
+  debugger_->publishProcessingTime();
 }
 
 void DetectionByTracker::divideUnderSegmentedObjects(

perception/map_based_prediction/README.md

@@ -218,11 +218,13 @@ If the target object is inside the road or crosswalk, this module outputs one or
 
 ### Output
 
-| Name                     | Type                                                   | Description                                                                           |
-| ------------------------ | ------------------------------------------------------ | ------------------------------------------------------------------------------------- |
-| `~/input/objects`        | `autoware_auto_perception_msgs::msg::TrackedObjects`   | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` |
-| `~/output/objects`       | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path.                                                 |
-| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray`                 | marker for visualization.                                                             |
+| Name                         | Type                                                   | Description                                                                           |
+| ---------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------------------------- |
+| `~/input/objects`            | `autoware_auto_perception_msgs::msg::TrackedObjects`   | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` |
+| `~/output/objects`           | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path.                                                 |
+| `~/objects_path_markers`     | `visualization_msgs::msg::MarkerArray`                 | marker for visualization.                                                             |
+| `~/debug/processing_time_ms` | `std_msgs::msg::Float64`                               | processing time of this module.                                                       |
+| `~/debug/cyclic_time_ms`     | `std_msgs::msg::Float64`                               | cyclic time of this module.                                                           |
 
 ## Parameters
 

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/debug_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>
@@ -33,7 +34,6 @@
 #include <geometry_msgs/msg/pose.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
-#include <tier4_debug_msgs/msg/string_stamped.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <lanelet2_core/Forward.h>
@@ -125,11 +125,14 @@ class MapBasedPredictionNode : public rclcpp::Node
   // ROS Publisher and Subscriber
   rclcpp::Publisher<PredictedObjects>::SharedPtr pub_objects_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_debug_markers_;
-  rclcpp::Publisher<StringStamped>::SharedPtr pub_calculation_time_;
   rclcpp::Subscription<TrackedObjects>::SharedPtr sub_objects_;
   rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
   rclcpp::Subscription<TrafficSignalArray>::SharedPtr sub_traffic_signals_;
 
+  // debug publisher
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
+
   // Object History
   std::unordered_map<std::string, std::deque<ObjectData>> objects_history_;
   std::map<std::pair<std::string, lanelet::Id>, rclcpp::Time> stopped_times_against_green_;
@@ -196,9 +199,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_;
-
   // Member Functions
   void mapCallback(const HADMapBin::ConstSharedPtr msg);
   void trafficSignalsCallback(const TrafficSignalArray::ConstSharedPtr msg);

perception/map_based_prediction/src/map_based_prediction_node.cpp

@@ -646,14 +646,6 @@ ObjectClassification::_label_type changeLabelForPrediction(
   }
 }
 
-StringStamped createStringStamped(const rclcpp::Time & now, const double data)
-{
-  StringStamped msg;
-  msg.stamp = now;
-  msg.data = std::to_string(data);
-  return msg;
-}
-
 // NOTE: These two functions are copied from the route_handler package.
 lanelet::Lanelets getRightOppositeLanelets(
   const std::shared_ptr<lanelet::LaneletMap> & lanelet_map_ptr,
@@ -816,10 +808,15 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
   pub_objects_ = this->create_publisher<PredictedObjects>("~/output/objects", rclcpp::QoS{1});
   pub_debug_markers_ =
     this->create_publisher<visualization_msgs::msg::MarkerArray>("maneuver", rclcpp::QoS{1});
-  pub_calculation_time_ = create_publisher<StringStamped>("~/debug/calculation_time", 1);
+  processing_time_publisher_ =
+    std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "map_based_prediction");
 
   set_param_res_ = this->add_on_set_parameters_callback(
     std::bind(&MapBasedPredictionNode::onParam, this, std::placeholders::_1));
+
+  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");
 }
 
 rcl_interfaces::msg::SetParametersResult MapBasedPredictionNode::onParam(
@@ -893,7 +890,7 @@ void MapBasedPredictionNode::trafficSignalsCallback(const TrafficSignalArray::Co
 
 void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPtr in_objects)
 {
-  stop_watch_.tic();
+  stop_watch_ptr_->toc("processing_time", true);
   // Guard for map pointer and frame transformation
   if (!lanelet_map_ptr_) {
     return;
@@ -1116,8 +1113,12 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt
   // Publish Results
   pub_objects_->publish(output);
   pub_debug_markers_->publish(debug_markers);
-  const auto calculation_time_msg = createStringStamped(now(), stop_watch_.toc());
-  pub_calculation_time_->publish(calculation_time_msg);
+  const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
+  const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
+  processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+    "debug/cyclic_time_ms", cyclic_time_ms);
+  processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+    "debug/processing_time_ms", processing_time_ms);
 }
 
 bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path)

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -148,7 +148,7 @@ void TrackerDebugger::publishProcessingTime()
     processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
       "debug/processing_time_ms", processing_time_ms);
     processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
-      "debug/elapsed_time_from_sensor_input_ms", elapsed_time_from_sensor_input_ * 1e3);
+      "debug/pipeline_latency_ms", elapsed_time_from_sensor_input_ * 1e3);
   }
 }
 

perception/object_merger/include/object_merger/node.hpp

@@ -18,6 +18,8 @@
 #include "object_merger/data_association/data_association.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 
@@ -81,6 +83,10 @@ class ObjectAssociationMergerNode : public rclcpp::Node
     std::map<int, double> generalized_iou_threshold;
     std::map<int /*class label*/, double /*distance_threshold*/> distance_threshold_map;
   } overlapped_judge_param_;
+
+  // debug publisher
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
 };
 }  // namespace object_association
 

perception/object_merger/src/object_association_merger/node.cpp

@@ -118,6 +118,13 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio
 
   merged_object_pub_ = create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
     "output/object", rclcpp::QoS{1});
+
+  // Debug publisher
+  processing_time_publisher_ =
+    std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "object_association_merger");
+  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");
 }
 
 void ObjectAssociationMergerNode::objectsCallback(
@@ -128,6 +135,7 @@ void ObjectAssociationMergerNode::objectsCallback(
   if (merged_object_pub_->get_subscription_count() < 1) {
     return;
   }
+  stop_watch_ptr_->toc("processing_time", true);
 
   /* transform to base_link coordinate */
   autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects0, transformed_objects1;
@@ -215,6 +223,11 @@ void ObjectAssociationMergerNode::objectsCallback(
 
   // publish output msg
   merged_object_pub_->publish(output_msg);
+  // 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));
 }
 }  // namespace object_association
 

perception/shape_estimation/src/node.cpp

@@ -50,10 +50,17 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option
   RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer);
   estimator_ =
     std::make_unique<ShapeEstimator>(use_corrector, use_filter, use_boost_bbox_optimizer);
+
+  processing_time_publisher_ =
+    std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "shape_estimation");
+  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");
 }
 
 void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg)
 {
+  stop_watch_ptr_->toc("processing_time", true);
   // Guard
   if (pub_->get_subscription_count() < 1) {
     return;
@@ -108,6 +115,11 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared
 
   // Publish
   pub_->publish(output_msg);
+
+  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));
 }
 
 #include <rclcpp_components/register_node_macro.hpp>

perception/shape_estimation/src/node.hpp

@@ -18,6 +18,8 @@
 #include "shape_estimation/shape_estimator.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <tier4_perception_msgs/msg/detected_objects_with_feature.hpp>
@@ -33,6 +35,10 @@ class ShapeEstimationNode : public rclcpp::Node
   rclcpp::Publisher<DetectedObjectsWithFeature>::SharedPtr pub_;
   rclcpp::Subscription<DetectedObjectsWithFeature>::SharedPtr sub_;
 
+  // debug publisher
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
+
   void callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg);
 
   std::unique_ptr<ShapeEstimator> estimator_;

perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp

@@ -20,6 +20,8 @@
 #include "tracking_object_merger/utils/utils.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
 #include <std_msgs/msg/header.hpp>
@@ -87,6 +89,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
     debug_object_pub_;
   bool publish_interpolated_sub_objects_;
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
 
   /* handle objects */
   std::unordered_map<MEASUREMENT_STATE, std::function<void(TrackedObject &, const TrackedObject &)>>

perception/tracking_object_merger/src/decorative_tracker_merger.cpp

@@ -151,6 +151,13 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio
   set3dDataAssociation("lidar-radar", data_association_map_);
   // radar-radar association matrix
   set3dDataAssociation("radar-radar", data_association_map_);
+
+  // debug publisher
+  processing_time_publisher_ =
+    std::make_unique<tier4_autoware_utils::DebugPublisher>(this, "decorative_object_merger_node");
+  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");
 }
 
 void DecorativeTrackerMergerNode::set3dDataAssociation(
@@ -182,6 +189,7 @@ void DecorativeTrackerMergerNode::set3dDataAssociation(
 void DecorativeTrackerMergerNode::mainObjectsCallback(
   const TrackedObjects::ConstSharedPtr & main_objects)
 {
+  stop_watch_ptr_->toc("processing_time", true);
   // try to merge sub object
   if (!sub_objects_buffer_.empty()) {
     // get interpolated sub objects
@@ -214,6 +222,10 @@ void DecorativeTrackerMergerNode::mainObjectsCallback(
   this->decorativeMerger(main_sensor_type_, main_objects);
 
   merged_object_pub_->publish(getTrackedObjects(main_objects->header));
+  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));
 }
 
 /**