perf(lidar_centerpoint): add cyclic and processing time (#1482)

* perf(lidar_centerpoint): add cyclic and processing time Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> * docs: update README Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>
tier4 · Aug 9, 2022 · e0816b2 · e0816b2
1 parent 5b8cb79
commit e0816b2
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 3 deletions.
diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md
@@ -20,9 +20,11 @@ We trained the models using <https://github.com/open-mmlab/mmdetection3d>.
 
 ### Output
 
-| Name               | Type                                                  | Description      |
-| ------------------ | ----------------------------------------------------- | ---------------- |
-| `~/output/objects` | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects |
+| Name                       | Type                                                  | Description          |
+| -------------------------- | ----------------------------------------------------- | -------------------- |
+| `~/output/objects`         | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects     |
+| `debug/cyclic_time_ms`     | `tier4_debug_msgs::msg::Float64Stamped`               | cyclic time (msg)    |
+| `debug/processing_time_ms` | `tier4_debug_msgs::msg::Float64Stamped`               | processing time (ms) |
 
 ## Parameters
 

diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/node.hpp
@@ -17,6 +17,8 @@
 
 #include <lidar_centerpoint/centerpoint_trt.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_object_kinematics.hpp>
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
@@ -51,6 +53,11 @@ class LidarCenterPointNode : public rclcpp::Node
   bool has_twist_{false};
 
   std::unique_ptr<CenterPointTRT> detector_ptr_{nullptr};
+
+  // debugger
+  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};
 };
 
 }  // namespace centerpoint

diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp
@@ -89,6 +89,16 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti
     std::bind(&LidarCenterPointNode::pointCloudCallback, this, std::placeholders::_1));
   objects_pub_ = this->create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
     "~/output/objects", rclcpp::QoS{1});
+
+  // initialize debug tool
+  {
+    using tier4_autoware_utils::DebugPublisher;
+    using tier4_autoware_utils::StopWatch;
+    stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
+    debug_publisher_ptr_ = std::make_unique<DebugPublisher>(this, "lidar_centerpoint");
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
 }
 
 void LidarCenterPointNode::pointCloudCallback(
@@ -100,6 +110,10 @@ void LidarCenterPointNode::pointCloudCallback(
     return;
   }
 
+  if (stop_watch_ptr_) {
+    stop_watch_ptr_->toc("processing_time", true);
+  }
+
   std::vector<Box3D> det_boxes3d;
   bool is_success = detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d);
   if (!is_success) {
@@ -117,6 +131,16 @@ void LidarCenterPointNode::pointCloudCallback(
   if (objects_sub_count > 0) {
     objects_pub_->publish(output_msg);
   }
+
+  // add processing time for debug
+  if (debug_publisher_ptr_ && stop_watch_ptr_) {
+    const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
+    const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
+    debug_publisher_ptr_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/cyclic_time_ms", cyclic_time_ms);
+    debug_publisher_ptr_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_time_ms", processing_time_ms);
+  }
 }
 
 }  // namespace centerpoint