From dabeaa640f25cd7af3ea3333a3fb4533cc64cf5f Mon Sep 17 00:00:00 2001 From: Masaki Baba Date: Tue, 10 Sep 2024 13:39:58 +0900 Subject: [PATCH] feat(ground_segmentation): add time_keeper (#8585) * add time_keeper Signed-off-by: a-maumau * add timekeeper option Signed-off-by: a-maumau * add autoware_universe_utils Signed-off-by: a-maumau * fix topic name Signed-off-by: a-maumau * add scope and timekeeper Signed-off-by: a-maumau * remove debug code Signed-off-by: a-maumau * remove some timekeeper and mod block comment Signed-off-by: a-maumau --------- Signed-off-by: a-maumau --- .../config/ground_segmentation.param.yaml | 3 + .../config/ransac_ground_filter.param.yaml | 3 + .../config/ray_ground_filter.param.yaml | 3 + .../config/scan_ground_filter.param.yaml | 3 + .../autoware_ground_segmentation/package.xml | 1 + .../src/ransac_ground_filter/node.cpp | 16 ++ .../src/ransac_ground_filter/node.hpp | 6 + .../src/ray_ground_filter/node.cpp | 23 +++ .../src/ray_ground_filter/node.hpp | 8 + .../src/scan_ground_filter/node.cpp | 156 ++++++++++++------ .../src/scan_ground_filter/node.hpp | 6 + .../test/test_ransac_ground_filter.cpp | 2 + .../test/test_ray_ground_filter.cpp | 3 + .../test/test_scan_ground_filter.cpp | 4 + 14 files changed, 184 insertions(+), 53 deletions(-) diff --git a/perception/autoware_ground_segmentation/config/ground_segmentation.param.yaml b/perception/autoware_ground_segmentation/config/ground_segmentation.param.yaml index 304dcc8990fe2..4fe68a19fa6e3 100644 --- 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 index 0d4a7d574499c..4b645dba95faf 100644 --- 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 index 5b9e8c06595f7..ac29f25ddb729 100644 --- 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 index 35d494a620b19..49c6c32624e75 100644 --- 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 index ff61a6b600f54..6b0ae57aa805a 100644 --- a/perception/autoware_ground_segmentation/package.xml +++ b/perception/autoware_ground_segmentation/package.xml @@ -21,6 +21,7 @@ ament_index_cpp autoware_pointcloud_preprocessor + autoware_universe_utils autoware_vehicle_info_utils libopencv-dev pcl_conversions diff --git a/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/ransac_ground_filter/node.cpp index e2f96d862e1a6..83e0538bd56de 100644 --- 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(this, has_static_tf_only_); + + bool use_time_keeper = declare_parameter("publish_processing_time_detail"); + if (use_time_keeper) { + detailed_processing_time_publisher_ = + this->create_publisher( + "~/debug/processing_time_detail_ms", 1); + auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); + } } void RANSACGroundFilterComponent::setDebugPublisher() @@ -204,6 +214,9 @@ void RANSACGroundFilterComponent::applyRANSAC( const pcl::PointCloud::Ptr & input, pcl::PointIndices::Ptr & output_inliers, pcl::ModelCoefficients::Ptr & output_coefficients) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + pcl::SACSegmentation seg; seg.setOptimizeCoefficients(true); seg.setRadiusLimits(0.3, std::numeric_limits::max()); @@ -219,6 +232,9 @@ void RANSACGroundFilterComponent::filter( const PointCloud2::ConstSharedPtr & input, [[maybe_unused]] const IndicesPtr & indices, PointCloud2 & output) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__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 index b36a7f5e55fc4..f9638086bf811 100644 --- 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 @@ -85,6 +86,11 @@ class RANSACGroundFilterComponent : public autoware::pointcloud_preprocessor::Fi Eigen::Vector3d unit_vec_ = Eigen::Vector3d::UnitZ(); std::unique_ptr managed_tf_buffer_{nullptr}; + // time keeper related + rclcpp::Publisher::SharedPtr + detailed_processing_time_publisher_; + std::shared_ptr 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 index fd8fcd4dc29e7..70ccffe2412a9 100644 --- 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 #include #include 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,6 +71,15 @@ 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("publish_processing_time_detail"); + if (use_time_keeper) { + detailed_processing_time_publisher_ = + this->create_publisher( + "~/debug/processing_time_detail_ms", 1); + auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); + } } void RayGroundFilterComponent::ConvertXYZIToRTZColor( @@ -76,6 +87,9 @@ void RayGroundFilterComponent::ConvertXYZIToRTZColor( std::vector & out_radial_divided_indices, std::vector & out_radial_ordered_clouds) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__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 & in_radial_ordered_clouds, pcl::PointIndices & out_ground_indices, pcl::PointIndices & out_no_ground_indices) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__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 st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__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 st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::scoped_lock lock(mutex_); pcl::PointCloud::Ptr current_sensor_cloud_ptr(new pcl::PointCloud); diff --git a/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/ray_ground_filter/node.hpp index 54de09b37ad96..f27d4c774e3fe 100644 --- 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 #include @@ -72,6 +74,7 @@ #include #include +#include #include #include @@ -140,6 +143,11 @@ class RayGroundFilterComponent : public autoware::pointcloud_preprocessor::Filte pcl::PointCloud::Ptr previous_cloud_ptr_; // holds the previous groundless result of // ground classification + // time keeper related + rclcpp::Publisher::SharedPtr + detailed_processing_time_publisher_; + std::shared_ptr 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/scan_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp index 40c00e082c60e..72481d13c41fb 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp @@ -30,6 +30,7 @@ using autoware::universe_utils::calcDistance3d; using autoware::universe_utils::deg2rad; using autoware::universe_utils::normalizeDegree; using autoware::universe_utils::normalizeRadian; +using autoware::universe_utils::ScopedTimeTrack; using autoware::vehicle_info_utils::VehicleInfoUtils; ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & options) @@ -85,6 +86,15 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & debug_publisher_ptr_ = std::make_unique(this, "scan_ground_filter"); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); + + bool use_time_keeper = declare_parameter("publish_processing_time_detail"); + if (use_time_keeper) { + detailed_processing_time_publisher_ = + this->create_publisher( + "~/debug/processing_time_detail_ms", 1); + auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); + } } } @@ -114,6 +124,9 @@ inline void ScanGroundFilterComponent::get_point_from_global_offset( void ScanGroundFilterComponent::convertPointcloudGridScan( const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + out_radial_ordered_points.resize(radial_dividers_num_); PointData current_point; @@ -128,46 +141,59 @@ void ScanGroundFilterComponent::convertPointcloudGridScan( const size_t in_cloud_data_size = in_cloud->data.size(); const size_t in_cloud_point_step = in_cloud->point_step; - size_t point_index = 0; - pcl::PointXYZ input_point; - for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; - global_offset += in_cloud_point_step) { - get_point_from_global_offset(in_cloud, input_point, global_offset); - - auto x{input_point.x - x_shift}; // base on front wheel center - auto radius{static_cast(std::hypot(x, input_point.y))}; - auto theta{normalizeRadian(std::atan2(x, input_point.y), 0.0)}; - - // divide by vertical angle - auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; - uint16_t grid_id = 0; - if (radius <= grid_mode_switch_radius_) { - grid_id = static_cast(radius * inv_grid_size_m); - } else { - auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; - grid_id = grid_id_offset + gamma * inv_grid_size_rad; + { // grouping pointcloud by its horizontal angle + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("horizontal_angle_grouping", *time_keeper_); + + size_t point_index = 0; + pcl::PointXYZ input_point; + for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; + global_offset += in_cloud_point_step) { + get_point_from_global_offset(in_cloud, input_point, global_offset); + + auto x{input_point.x - x_shift}; // base on front wheel center + auto radius{static_cast(std::hypot(x, input_point.y))}; + auto theta{normalizeRadian(std::atan2(x, input_point.y), 0.0)}; + + // divide by vertical angle + auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; + uint16_t grid_id = 0; + if (radius <= grid_mode_switch_radius_) { + grid_id = static_cast(radius * inv_grid_size_m); + } else { + auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; + grid_id = grid_id_offset + gamma * inv_grid_size_rad; + } + current_point.grid_id = grid_id; + current_point.radius = radius; + current_point.point_state = PointLabel::INIT; + current_point.orig_index = point_index; + + // radial divisions + out_radial_ordered_points[radial_div].emplace_back(current_point); + ++point_index; } - current_point.grid_id = grid_id; - current_point.radius = radius; - current_point.point_state = PointLabel::INIT; - current_point.orig_index = point_index; - - // radial divisions - out_radial_ordered_points[radial_div].emplace_back(current_point); - ++point_index; } - // sort by distance - for (size_t i = 0; i < radial_dividers_num_; ++i) { - std::sort( - out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), - [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); + { // sorting pointcloud by distance, on each horizontal angle group + std::unique_ptr inner_st_ptr; + if (time_keeper_) inner_st_ptr = std::make_unique("sort", *time_keeper_); + + for (size_t i = 0; i < radial_dividers_num_; ++i) { + std::sort( + out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), + [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); + } } } void ScanGroundFilterComponent::convertPointcloud( const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + out_radial_ordered_points.resize(radial_dividers_num_); PointData current_point; @@ -176,30 +202,41 @@ void ScanGroundFilterComponent::convertPointcloud( const size_t in_cloud_data_size = in_cloud->data.size(); const size_t in_cloud_point_step = in_cloud->point_step; - size_t point_index = 0; - pcl::PointXYZ input_point; - for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; - global_offset += in_cloud_point_step) { - // Point - get_point_from_global_offset(in_cloud, input_point, global_offset); - - auto radius{static_cast(std::hypot(input_point.x, input_point.y))}; - auto theta{normalizeRadian(std::atan2(input_point.x, input_point.y), 0.0)}; - auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; + { // grouping pointcloud by its horizontal angle + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("horizontal_angle_grouping", *time_keeper_); + + size_t point_index = 0; + pcl::PointXYZ input_point; + for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; + global_offset += in_cloud_point_step) { + // Point + get_point_from_global_offset(in_cloud, input_point, global_offset); + + auto radius{static_cast(std::hypot(input_point.x, input_point.y))}; + auto theta{normalizeRadian(std::atan2(input_point.x, input_point.y), 0.0)}; + auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; + + current_point.radius = radius; + current_point.point_state = PointLabel::INIT; + current_point.orig_index = point_index; + + // radial divisions + out_radial_ordered_points[radial_div].emplace_back(current_point); + ++point_index; + } + } - current_point.radius = radius; - current_point.point_state = PointLabel::INIT; - current_point.orig_index = point_index; + { // sorting pointcloud by distance, on each horizontal angle group + std::unique_ptr inner_st_ptr; + if (time_keeper_) inner_st_ptr = std::make_unique("sort", *time_keeper_); - // radial divisions - out_radial_ordered_points[radial_div].emplace_back(current_point); - ++point_index; - } - // sort by distance - for (size_t i = 0; i < radial_dividers_num_; ++i) { - std::sort( - out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), - [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); + for (size_t i = 0; i < radial_dividers_num_; ++i) { + std::sort( + out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), + [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); + } } } @@ -336,6 +373,9 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + out_no_ground_indices.indices.clear(); for (size_t i = 0; i < in_radial_ordered_clouds.size(); ++i) { PointsCentroid ground_cluster; @@ -456,6 +496,9 @@ void ScanGroundFilterComponent::classifyPointCloud( const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + out_no_ground_indices.indices.clear(); const pcl::PointXYZ init_ground_point(0, 0, 0); @@ -470,6 +513,7 @@ void ScanGroundFilterComponent::classifyPointCloud( PointsCentroid ground_cluster, non_ground_cluster; PointLabel prev_point_label = PointLabel::INIT; pcl::PointXYZ prev_gnd_point(0, 0, 0), p_orig_point, prev_p_orig_point; + // loop through each point in the radial div for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { float points_distance = 0.0f; @@ -569,6 +613,9 @@ void ScanGroundFilterComponent::extractObjectPoints( const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, PointCloud2 & out_object_cloud) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + size_t output_data_size = 0; for (const auto & i : in_indices.indices) { @@ -584,6 +631,9 @@ void ScanGroundFilterComponent::faster_filter( PointCloud2 & output, [[maybe_unused]] const autoware::pointcloud_preprocessor::TransformInfo & transform_info) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp index e87fd0c341a6f..12590afd5fb86 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.hpp @@ -17,6 +17,7 @@ #include "autoware/pointcloud_preprocessor/filter.hpp" #include "autoware/pointcloud_preprocessor/transform_info.hpp" +#include "autoware/universe_utils/system/time_keeper.hpp" #include "autoware_vehicle_info_utils/vehicle_info.hpp" #include @@ -201,6 +202,11 @@ class ScanGroundFilterComponent : public autoware::pointcloud_preprocessor::Filt size_t radial_dividers_num_; VehicleInfo vehicle_info_; + // time keeper related + rclcpp::Publisher::SharedPtr + detailed_processing_time_publisher_; + std::shared_ptr 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/test/test_ransac_ground_filter.cpp b/perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp index e18c166adb8b3..a3f8b9d4b19b3 100644 --- a/perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp +++ b/perception/autoware_ground_segmentation/test/test_ransac_ground_filter.cpp @@ -152,6 +152,7 @@ TEST_F(RansacGroundFilterTestSuite, TestCase1) double voxel_size_z = params["voxel_size_z"].as(); double height_threshold = params["height_threshold"].as(); bool debug = params["debug"].as(); + bool publish_processing_time_detail = params["publish_processing_time_detail"].as(); const auto pcd_path = share_dir + "/data/test.pcd"; pcl::PointCloud cloud; @@ -191,6 +192,7 @@ TEST_F(RansacGroundFilterTestSuite, TestCase1) parameters.emplace_back("voxel_size_z", voxel_size_z); parameters.emplace_back("height_threshold", height_threshold); parameters.emplace_back("debug", debug); + parameters.emplace_back("publish_processing_time_detail", publish_processing_time_detail); rclcpp::NodeOptions node_options; node_options.parameter_overrides(parameters); diff --git a/perception/autoware_ground_segmentation/test/test_ray_ground_filter.cpp b/perception/autoware_ground_segmentation/test/test_ray_ground_filter.cpp index 81efeff8111cb..1e494f683f181 100644 --- a/perception/autoware_ground_segmentation/test/test_ray_ground_filter.cpp +++ b/perception/autoware_ground_segmentation/test/test_ray_ground_filter.cpp @@ -121,6 +121,7 @@ TEST_F(RayGroundFilterComponentTestSuite, TestCase1) double min_height_threshold_ = params["min_height_threshold"].as(); double concentric_divider_distance_ = params["concentric_divider_distance"].as(); double reclass_distance_threshold_ = params["reclass_distance_threshold"].as(); + bool publish_processing_time_detail_ = params["publish_processing_time_detail"].as(); const auto pcd_path = share_dir + "/data/test.pcd"; pcl::PointCloud cloud; @@ -167,6 +168,8 @@ TEST_F(RayGroundFilterComponentTestSuite, TestCase1) parameters.emplace_back(rclcpp::Parameter("min_y", min_y_)); parameters.emplace_back(rclcpp::Parameter("max_y", max_y_)); parameters.emplace_back(rclcpp::Parameter("use_vehicle_footprint", use_vehicle_footprint_)); + parameters.emplace_back( + rclcpp::Parameter("publish_processing_time_detail", publish_processing_time_detail_)); node_options.parameter_overrides(parameters); auto ray_ground_filter_test = std::make_shared(node_options); diff --git a/perception/autoware_ground_segmentation/test/test_scan_ground_filter.cpp b/perception/autoware_ground_segmentation/test/test_scan_ground_filter.cpp index c6603dfd9a3e2..706ffff3ce511 100644 --- a/perception/autoware_ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/autoware_ground_segmentation/test/test_scan_ground_filter.cpp @@ -122,6 +122,8 @@ class ScanGroundFilterTest : public ::testing::Test parameters.emplace_back( rclcpp::Parameter("use_recheck_ground_cluster", use_recheck_ground_cluster_)); parameters.emplace_back(rclcpp::Parameter("use_lowest_point", use_lowest_point_)); + parameters.emplace_back( + rclcpp::Parameter("publish_processing_time_detail", publish_processing_time_detail_)); options.parameter_overrides(parameters); @@ -200,6 +202,7 @@ class ScanGroundFilterTest : public ::testing::Test radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as(); use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as(); use_lowest_point_ = params["use_lowest_point"].as(); + publish_processing_time_detail_ = params["publish_processing_time_detail"].as(); } float global_slope_max_angle_deg_ = 0.0; @@ -218,6 +221,7 @@ class ScanGroundFilterTest : public ::testing::Test float radial_divider_angle_deg_; bool use_recheck_ground_cluster_; bool use_lowest_point_; + bool publish_processing_time_detail_; }; TEST_F(ScanGroundFilterTest, TestCase1)