diff --git a/sensing/pointcloud_preprocessor/README.md b/sensing/pointcloud_preprocessor/README.md index 91ef34f4cae72..5c6402efdf23d 100644 --- a/sensing/pointcloud_preprocessor/README.md +++ b/sensing/pointcloud_preprocessor/README.md @@ -56,7 +56,45 @@ Detail description of each filter's algorithm is in the following links. ## Assumptions / Known limits -`pointcloud_preprocessor::Filter` is implemented based on pcl_perception [1] because of [this issue](https://github.com/ros-perception/perception_pcl/issues/9). +`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 pipeline latency 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 pipeline latency 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 pipeline latency times for each node in the pipeline by subscribing to the +following ROS 2 topics: + +- `/sensing/lidar/LidarX/crop_box_filter_self/debug/pipeline_latency_ms` +- `/sensing/lidar/LidarX/crop_box_filter_mirror/debug/pipeline_latency_ms` +- `/sensing/lidar/LidarX/distortion_corrector/debug/pipeline_latency_ms` +- `/sensing/lidar/LidarX/ring_outlier_filter/debug/pipeline_latency_ms` +- `/sensing/lidar/concatenate_data_synchronizer/debug/sensing/lidar/LidarX/pointcloud/pipeline_latency_ms` + +These topics provide the pipeline latency 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 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..635e0d1103f89 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 pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); + } + } + } + // 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..cfbeffee9982c 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 pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) + .count(); + + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index dd38b85a2b56d..d1d91ed7ec439 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 pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - points_msg->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + } + 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 d2570b9c4d786..d968b06a0dc61 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 @@ -204,6 +204,14 @@ void RingOutlierFilterComponent::faster_filter( "debug/cyclic_time_ms", cyclic_time_ms); debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); + + auto pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) + .count(); + + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); } }