feat(ground_segmentation): add time_keeper (#8585)

* add time_keeper Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * add timekeeper option Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * add autoware_universe_utils Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * fix topic name Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * add scope and timekeeper Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * remove debug code Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> * remove some timekeeper and mod block comment Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com> --------- Signed-off-by: a-maumau <maumaumaumaumaumaumaumaumaumau@gmail.com>
autowarefoundation · Sep 10, 2024 · dabeaa6 · dabeaa6
1 parent a1cba1f
commit dabeaa6
Show file tree

Hide file tree

Showing 14 changed files with 184 additions and 53 deletions.
diff --git a/perception/autoware_ground_segmentation/config/ground_segmentation.param.yaml b/perception/autoware_ground_segmentation/config/ground_segmentation.param.yaml
@@ -19,3 +19,6 @@
         radial_divider_angle_deg: 1.0
         use_recheck_ground_cluster: true
         use_lowest_point: true
+
+        # debug parameters
+        publish_processing_time_detail: false
diff --git a/perception/autoware_ground_segmentation/config/ransac_ground_filter.param.yaml b/perception/autoware_ground_segmentation/config/ransac_ground_filter.param.yaml
@@ -12,3 +12,6 @@
     voxel_size_z: 0.04
     height_threshold: 0.01
     debug: false
+
+    # debug parameters
+    publish_processing_time_detail: false
diff --git a/perception/autoware_ground_segmentation/config/ray_ground_filter.param.yaml b/perception/autoware_ground_segmentation/config/ray_ground_filter.param.yaml
@@ -12,3 +12,6 @@
       min_height_threshold: 0.15
       concentric_divider_distance: 0.0
       reclass_distance_threshold: 0.1
+
+      # debug parameters
+      publish_processing_time_detail: false
diff --git a/perception/autoware_ground_segmentation/config/scan_ground_filter.param.yaml b/perception/autoware_ground_segmentation/config/scan_ground_filter.param.yaml
@@ -16,3 +16,6 @@
     radial_divider_angle_deg: 1.0
     use_recheck_ground_cluster: true
     use_lowest_point: true
+
+    # debug parameters
+    publish_processing_time_detail: false
diff --git a/perception/autoware_ground_segmentation/package.xml b/perception/autoware_ground_segmentation/package.xml
@@ -21,6 +21,7 @@
 
   <depend>ament_index_cpp</depend>
   <depend>autoware_pointcloud_preprocessor</depend>
+  <depend>autoware_universe_utils</depend>
   <depend>autoware_vehicle_info_utils</depend>
   <depend>libopencv-dev</depend>
   <depend>pcl_conversions</depend>

diff --git a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp
@@ -81,6 +81,7 @@ PlaneBasis getPlaneBasis(const Eigen::Vector3d & plane_normal)
 }
 
 using autoware::pointcloud_preprocessor::get_param;
+using autoware::universe_utils::ScopedTimeTrack;
 
 RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options)
 : Filter("RANSACGroundFilter", options)
@@ -118,6 +119,15 @@ RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptio
 
   managed_tf_buffer_ =
     std::make_unique<autoware::universe_utils::ManagedTransformBuffer>(this, has_static_tf_only_);
+
+  bool use_time_keeper = declare_parameter<bool>("publish_processing_time_detail");
+  if (use_time_keeper) {
+    detailed_processing_time_publisher_ =
+      this->create_publisher<autoware::universe_utils::ProcessingTimeDetail>(
+        "~/debug/processing_time_detail_ms", 1);
+    auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_);
+    time_keeper_ = std::make_shared<autoware::universe_utils::TimeKeeper>(time_keeper);
+  }
 }
 
 void RANSACGroundFilterComponent::setDebugPublisher()
@@ -204,6 +214,9 @@ void RANSACGroundFilterComponent::applyRANSAC(
   const pcl::PointCloud<PointType>::Ptr & input, pcl::PointIndices::Ptr & output_inliers,
   pcl::ModelCoefficients::Ptr & output_coefficients)
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   pcl::SACSegmentation<PointType> seg;
   seg.setOptimizeCoefficients(true);
   seg.setRadiusLimits(0.3, std::numeric_limits<double>::max());
@@ -219,6 +232,9 @@ void RANSACGroundFilterComponent::filter(
   const PointCloud2::ConstSharedPtr & input, [[maybe_unused]] const IndicesPtr & indices,
   PointCloud2 & output)
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   std::scoped_lock lock(mutex_);
   sensor_msgs::msg::PointCloud2::SharedPtr input_transformed_ptr(new sensor_msgs::msg::PointCloud2);
   if (!transformPointCloud(base_frame_, input, input_transformed_ptr)) {

diff --git a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.hpp
@@ -16,6 +16,7 @@
 #define RANSAC_GROUND_FILTER__NODE_HPP_
 
 #include "autoware/pointcloud_preprocessor/filter.hpp"
+#include "autoware/universe_utils/system/time_keeper.hpp"
 
 #include <autoware/universe_utils/ros/managed_transform_buffer.hpp>
 
@@ -85,6 +86,11 @@ class RANSACGroundFilterComponent : public autoware::pointcloud_preprocessor::Fi
   Eigen::Vector3d unit_vec_ = Eigen::Vector3d::UnitZ();
   std::unique_ptr<autoware::universe_utils::ManagedTransformBuffer> managed_tf_buffer_{nullptr};
 
+  // time keeper related
+  rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr
+    detailed_processing_time_publisher_;
+  std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_;
+
   /*!
    * Output transformed PointCloud from in_cloud_ptr->header.frame_id to in_target_frame
    * @param[in] in_target_frame Coordinate system to perform transform

diff --git a/perception/autoware_ground_segmentation/src/ray_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/ray_ground_filter/node.cpp
@@ -31,12 +31,14 @@
 
 #include "node.hpp"
 
+#include <memory>
 #include <string>
 #include <vector>
 
 namespace autoware::ground_segmentation
 {
 using autoware::pointcloud_preprocessor::get_param;
+using autoware::universe_utils::ScopedTimeTrack;
 
 RayGroundFilterComponent::RayGroundFilterComponent(const rclcpp::NodeOptions & options)
 : Filter("RayGroundFilter", options)
@@ -69,13 +71,25 @@ RayGroundFilterComponent::RayGroundFilterComponent(const rclcpp::NodeOptions & o
   using std::placeholders::_1;
   set_param_res_ = this->add_on_set_parameters_callback(
     std::bind(&RayGroundFilterComponent::paramCallback, this, _1));
+
+  bool use_time_keeper = declare_parameter<bool>("publish_processing_time_detail");
+  if (use_time_keeper) {
+    detailed_processing_time_publisher_ =
+      this->create_publisher<autoware::universe_utils::ProcessingTimeDetail>(
+        "~/debug/processing_time_detail_ms", 1);
+    auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_);
+    time_keeper_ = std::make_shared<autoware::universe_utils::TimeKeeper>(time_keeper);
+  }
 }
 
 void RayGroundFilterComponent::ConvertXYZIToRTZColor(
   const pcl::PointCloud<PointType_>::Ptr in_cloud, PointCloudXYZRTColor & out_organized_points,
   std::vector<pcl::PointIndices> & out_radial_divided_indices,
   std::vector<PointCloudXYZRTColor> & out_radial_ordered_clouds)
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   out_organized_points.resize(in_cloud->points.size());
   out_radial_divided_indices.clear();
   out_radial_divided_indices.resize(radial_dividers_num_);
@@ -153,6 +167,9 @@ void RayGroundFilterComponent::ClassifyPointCloud(
   std::vector<PointCloudXYZRTColor> & in_radial_ordered_clouds,
   pcl::PointIndices & out_ground_indices, pcl::PointIndices & out_no_ground_indices)
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   out_ground_indices.indices.clear();
   out_no_ground_indices.indices.clear();
 #pragma omp for
@@ -275,6 +292,9 @@ void RayGroundFilterComponent::ExtractPointsIndices(
   const PointCloud2::ConstSharedPtr in_cloud_ptr, const pcl::PointIndices & in_indices,
   PointCloud2::SharedPtr ground_cloud_msg_ptr, PointCloud2::SharedPtr no_ground_cloud_msg_ptr)
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   initializePointCloud2(in_cloud_ptr, ground_cloud_msg_ptr);
   initializePointCloud2(in_cloud_ptr, no_ground_cloud_msg_ptr);
   int point_step = in_cloud_ptr->point_step;
@@ -312,6 +332,9 @@ void RayGroundFilterComponent::filter(
   const PointCloud2::ConstSharedPtr & input, [[maybe_unused]] const IndicesPtr & indices,
   PointCloud2 & output)
 {
+  std::unique_ptr<ScopedTimeTrack> st_ptr;
+  if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
+
   std::scoped_lock lock(mutex_);
 
   pcl::PointCloud<PointType_>::Ptr current_sensor_cloud_ptr(new pcl::PointCloud<PointType_>);

diff --git a/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp
@@ -45,6 +45,8 @@
 #ifndef RAY_GROUND_FILTER__NODE_HPP_
 #define RAY_GROUND_FILTER__NODE_HPP_
 
+#include "autoware/universe_utils/system/time_keeper.hpp"
+
 #include <sensor_msgs/msg/point_cloud2.hpp>
 
 #include <pcl/filters/extract_indices.h>
@@ -72,6 +74,7 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <chrono>
+#include <memory>
 #include <string>
 #include <vector>
 
@@ -140,6 +143,11 @@ class RayGroundFilterComponent : public autoware::pointcloud_preprocessor::Filte
   pcl::PointCloud<PointType_>::Ptr previous_cloud_ptr_;  // holds the previous groundless result of
                                                          // ground classification
 
+  // time keeper related
+  rclcpp::Publisher<autoware::universe_utils::ProcessingTimeDetail>::SharedPtr
+    detailed_processing_time_publisher_;
+  std::shared_ptr<autoware::universe_utils::TimeKeeper> time_keeper_;
+
   /*!
    * Output transformed PointCloud from in_cloud_ptr->header.frame_id to in_target_frame
    * @param[in] in_target_frame Coordinate system to perform transform