diff --git a/sensing/pointcloud_preprocessor/README.md b/sensing/pointcloud_preprocessor/README.md index 91ef34f4cae72..21a92cbddabb4 100644 --- a/sensing/pointcloud_preprocessor/README.md +++ b/sensing/pointcloud_preprocessor/README.md @@ -58,6 +58,45 @@ Detail description of each filter's algorithm is in the following links. `pointcloud_preprocessor::Filter` is implemented based on pcl_perception [1] because of [this issue](https://github.com/ros-perception/perception_pcl/issues/9). +## Measuring the performance + +In Autoware, point cloud data from each LiDAR sensor undergoes preprocessing in the sensing pipeline before being input +into the perception pipeline. The preprocessing stages are illustrated in the diagram below: + +![pointcloud_preprocess_pipeline.drawio.png](docs%2Fimage%2Fpointcloud_preprocess_pipeline.drawio.png) + +Each stage in the pipeline incurs a processing delay. Mostly, we've used `ros2 topic delay /topic_name` to measure +the time between the message header and the current time. This approach works well for small-sized messages. However, +when dealing with large point cloud messages, this method introduces an additional delay. This is primarily because +accessing these large point cloud messages externally impacts the pipeline's performance. + +Our sensing/perception nodes are designed to run within composable node containers, leveraging intra-process +communication. External subscriptions to these messages (like using ros2 topic delay or rviz2) impose extra delays and +can even slow down the pipeline by subscribing externally. Therefore, these measurements will not be accurate. + +To mitigate this issue, we've adopted a method where each node in the pipeline reports its accumulated processing time. +This approach ensures the integrity of intra-process communication and provides a more accurate measure of delays in the +pipeline. + +### Benchmarking The Pipeline + +The nodes within the pipeline report the accumulated time, indicating the duration from the sensor driver's pointcloud +output to the node's output. This data is crucial for assessing the pipeline's health and efficiency. + +When running Autoware, you can monitor the accumulated times for each node in the pipeline by subscribing to the +following ROS2 topics: + +``` +/sensing/lidar/LidarX/crop_box_filter_self/debug/accumulated_time_ms +/sensing/lidar/LidarX/crop_box_filter_mirror/debug/accumulated_time_ms +/sensing/lidar/LidarX/distortion_corrector/debug/accumulated_time_ms +/sensing/lidar/LidarX/ring_outlier_filter/debug/accumulated_time_ms +/sensing/lidar/concatenate_data_synchronizer/debug/accumulated_time_ms/sensing/lidar/LidarX/pointcloud +``` + +These topics provide the accumulated processing times, giving insights into the delays at various stages of the pipeline +from the sensor output of LidarX to each subsequent node. + ## (Optional) Error detection and handling ## (Optional) Performance characterization diff --git a/sensing/pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png b/sensing/pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png new file mode 100644 index 0000000000000..e690d0179afa6 Binary files /dev/null and b/sensing/pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png differ diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index ff72e433e9602..d58b105215bc6 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -380,6 +380,20 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() const auto & transformed_raw_points = PointCloudConcatenateDataSynchronizerComponent::combineClouds(concat_cloud_ptr); + for (const auto & e : cloud_stdmap_) { + if (e.second != nullptr) { + if (debug_publisher_) { + const auto accumulated_time = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/accumulated_time_ms" + e.first, accumulated_time); + } + } + } + // publish concatenated pointcloud if (concat_cloud_ptr) { auto output = std::make_unique(*concat_cloud_ptr); 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 20d1f5c8b3d6d..236eb3370dfbb 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 @@ -65,7 +65,7 @@ CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & optio using tier4_autoware_utils::DebugPublisher; using tier4_autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ = std::make_unique(this, "crop_box_filter"); + debug_publisher_ = std::make_unique(this, this->get_name()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -195,6 +195,14 @@ void CropBoxFilterComponent::faster_filter( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + + auto accumulated_time = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) + .count(); + + debug_publisher_->publish( + "debug/accumulated_time_ms", accumulated_time); } } diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index dd38b85a2b56d..3f0af79bf5418 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -128,6 +128,16 @@ void DistortionCorrectorComponent::onPointCloud(PointCloud2::UniquePtr points_ms undistortPointCloud(tf2_base_link_to_sensor, *points_msg); + if (debug_publisher_) { + auto accumulated_time = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - points_msg->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/accumulated_time_ms", accumulated_time); + } + undistorted_points_pub_->publish(std::move(points_msg)); // add processing time for debug 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 e277b221a090d..9495940bc501f 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 @@ -346,6 +346,14 @@ void RingOutlierFilterComponent::faster_filter( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + + auto accumulated_time = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) + .count(); + + debug_publisher_->publish( + "debug/accumulated_time_ms", accumulated_time); } }