perf(probabilistic_occupancy_grid_map): add cyclic and processing tim…

…e to pointcloud_based_occupancy_grid_map (autowarefoundation#2270) Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp> Signed-off-by: yukke42 <yusuke.muramatsu@tier4.jp>
YoshiRi · Nov 11, 2022 · ed1ea4b · ed1ea4b
1 parent fd7add5
commit ed1ea4b
Show file tree

Hide file tree

Showing 2 changed files with 27 additions and 0 deletions.
diff --git a/.../include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/.../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/..._map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/..._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(