From ed1ea4bfc241dbdaeb4c21c69f213c00d3c9b27a Mon Sep 17 00:00:00 2001
From: Yusuke Muramatsu <yukke42@users.noreply.github.com>
Date: Fri, 11 Nov 2022 17:48:01 +0900
Subject: [PATCH] perf(probabilistic_occupancy_grid_map): add cyclic and
 processing time to pointcloud_based_occupancy_grid_map (#2270)

Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>

Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>
---
 ...intcloud_based_occupancy_grid_map_node.hpp |  4 ++++
 ...intcloud_based_occupancy_grid_map_node.cpp | 23 +++++++++++++++++++
 2 files changed, 27 insertions(+)

diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp
index 8969b67cf2686..308d93ea5d7cc 100644
--- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp
+++ b/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp
@@ -22,6 +22,8 @@
 #include <builtin_interfaces/msg/time.hpp>
 #include <laser_geometry/laser_geometry.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <sensor_msgs/msg/laser_scan.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
@@ -65,6 +67,8 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node
   rclcpp::Publisher<OccupancyGrid>::SharedPtr occupancy_grid_map_pub_;
   message_filters::Subscriber<PointCloud2> obstacle_pointcloud_sub_;
   message_filters::Subscriber<PointCloud2> raw_pointcloud_sub_;
+  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_{};
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_ptr_{};
 
   std::shared_ptr<Buffer> tf2_{std::make_shared<Buffer>(get_clock())};
   std::shared_ptr<TransformListener> tf2_listener_{std::make_shared<TransformListener>(*tf2_)};
diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp
index 2459e2e0c0439..451f34e128523 100644
--- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp
+++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp
@@ -131,12 +131,26 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode(
   /* Occupancy grid */
   occupancy_grid_map_updater_ptr_ = std::make_shared<OccupancyGridMapBBFUpdater>(
     map_length / map_resolution, map_length / map_resolution, map_resolution);
+
+  // 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, "pointcloud_based_occupancy_grid_map");
+    stop_watch_ptr_->tic("cyclic_time");
+    stop_watch_ptr_->tic("processing_time");
+  }
 }
 
 void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw(
   const PointCloud2::ConstSharedPtr & input_obstacle_msg,
   const PointCloud2::ConstSharedPtr & input_raw_msg)
 {
+  if (stop_watch_ptr_) {
+    stop_watch_ptr_->toc("processing_time", true);
+  }
   // Apply height filter
   PointCloud2 cropped_obstacle_pc{};
   PointCloud2 cropped_raw_pc{};
@@ -187,6 +201,15 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw(
     occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr(
       map_frame_, input_raw_msg->header.stamp, pose.position.z, *occupancy_grid_map_updater_ptr_));
   }
+
+  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);
+  }
 }
 
 OccupancyGrid::UniquePtr PointcloudBasedOccupancyGridMapNode::OccupancyGridMapToMsgPtr(