From 1f1ec794481393725bd0803bedb9996a33597b05 Mon Sep 17 00:00:00 2001 From: tanaka3 Date: Thu, 12 May 2022 17:35:49 +0900 Subject: [PATCH] chore(pcd): add processing debug ms Signed-off-by: tanaka3 --- .../config/processing_time_ms.xml | 68 +++++++++++++++++++ .../crop_box_filter_nodelet.hpp | 5 ++ .../ring_outlier_filter_nodelet.hpp | 5 ++ .../lanelet2_map_filter_nodelet.hpp | 6 ++ .../crop_box_filter_nodelet.cpp | 12 ++++ .../ring_outlier_filter_nodelet.cpp | 12 ++++ 6 files changed, 108 insertions(+) create mode 100644 sensing/pointcloud_preprocessor/config/processing_time_ms.xml diff --git a/sensing/pointcloud_preprocessor/config/processing_time_ms.xml b/sensing/pointcloud_preprocessor/config/processing_time_ms.xml new file mode 100644 index 0000000000000..44a1a3f8c7046 --- /dev/null +++ b/sensing/pointcloud_preprocessor/config/processing_time_ms.xml @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp index 66e2c089a934f..3a3cbcfda36ca 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/crop_box_filter/crop_box_filter_nodelet.hpp @@ -56,6 +56,8 @@ #include "pointcloud_preprocessor/filter.hpp" #include +#include +#include #include @@ -88,6 +90,9 @@ class CropBoxFilterComponent : public pointcloud_preprocessor::Filter /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + /** \brief Parameter service callback */ rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index b0697ca106533..1a2d0ead481c6 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -17,6 +17,8 @@ #include "autoware_point_types/types.hpp" #include "pointcloud_preprocessor/filter.hpp" +#include +#include #include @@ -45,6 +47,9 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter /** \brief Parameter service callback */ rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + std::unique_ptr> stop_watch_ptr_; + std::unique_ptr debug_publisher_; + bool isCluster( sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::vector & tmp_indices) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp index 59ce02d8439a0..2633c219a0b8a 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp @@ -22,6 +22,8 @@ #include #include +#include +#include #include #include @@ -62,6 +64,10 @@ class Lanelet2MapFilterComponent : public rclcpp::Node float voxel_size_x_; float voxel_size_y_; + std::unique_ptr processing_time_publisher_ptr_; + std::unique_ptr> stop_watch_ptr_; + + void pointcloudCallback(const PointCloud2ConstPtr msg); void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index 52f092b6bc89a..c3357812a467c 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -60,6 +60,12 @@ namespace pointcloud_preprocessor CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & options) : Filter("CropBoxFilter", options) { + // initialize debug tool + { + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "crop_box_filter"); + } + // set initial parameters { auto & p = param_; @@ -91,6 +97,7 @@ void CropBoxFilterComponent::filter( PointCloud2 & output) { boost::mutex::scoped_lock lock(mutex_); + stop_watch_ptr_->tic(); output.data.resize(input->data.size()); Eigen::Vector3f pt(Eigen::Vector3f::Zero()); @@ -132,6 +139,11 @@ void CropBoxFilterComponent::filter( output.row_step = static_cast(output.data.size() / output.height); publishCropBoxPolygon(); + // add processing time for debug + { + const double processing_time_ms = stop_watch_ptr_->toc(true); + debug_publisher_->publish("debug/processing_time_ms", processing_time_ms); + } } void CropBoxFilterComponent::publishCropBoxPolygon() diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index 7a842665e5f14..c1f13aafb48b3 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -21,6 +21,12 @@ namespace pointcloud_preprocessor RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options) : Filter("RingOutlierFilter", options) { + // initialize debug tool + { + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "ring_outlier_filter"); + } + // set initial parameters { distance_ratio_ = static_cast(declare_parameter("distance_ratio", 1.03)); @@ -39,6 +45,7 @@ void RingOutlierFilterComponent::filter( PointCloud2 & output) { boost::mutex::scoped_lock lock(mutex_); + stop_watch_ptr_->tic(); std::unordered_map> input_ring_map; input_ring_map.reserve(128); sensor_msgs::msg::PointCloud2::SharedPtr input_ptr = @@ -49,7 +56,12 @@ void RingOutlierFilterComponent::filter( for (std::size_t idx = 0U; idx < input_ptr->data.size(); idx += input_ptr->point_step) { input_ring_map[*reinterpret_cast(&input_ptr->data[idx + ring_offset])].push_back( idx); + // add processing time for debug + { + const double processing_time_ms = stop_watch_ptr_->toc(true); + debug_publisher_->publish("debug/processing_time_ms", processing_time_ms); } +} PointCloud2Modifier output_modifier{output, input->header.frame_id}; output_modifier.reserve(input->width);