Merge pull request #1289 from tier4/feat/v0.19.1_performance_test_0510

feat(X2 performance tuning): merge pr about concatenate_data, downsample_filter, occupancy_grid_map

Author: 0x126

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -107,6 +107,25 @@
     description="if use_radar_tracking_fusion:=true, radar information is merged in tracking launch. Otherwise, radar information is merged in detection launch."
   />
 
+  <!-- Downsample pointcloud for perception usage -->
+  <arg name="downsample_input_pointcloud" default="false"/>
+  <arg name="downsample_voxel_size" default="0.05"/>
+  <let name="downsampled_pointcloud" value="/perception/common/pointcloud"/>
+  <let name="perception_pointcloud" value="$(var input/pointcloud)" unless="$(var downsample_input_pointcloud)"/>
+  <let name="perception_pointcloud" value="$(var downsampled_pointcloud)" if="$(var downsample_input_pointcloud)"/>
+  <group if="$(var downsample_input_pointcloud)">
+    <load_composable_node target="$(var pointcloud_container_name)">
+      <composable_node pkg="pointcloud_preprocessor" plugin="pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent" name="perception_input_pc_downsample_node" namespace="">
+        <remap from="input" to="$(var input/pointcloud)"/>
+        <remap from="output" to="$(var downsampled_pointcloud)"/>
+        <param name="voxel_size_x" value="$(var downsample_voxel_size)"/>
+        <param name="voxel_size_y" value="$(var downsample_voxel_size)"/>
+        <param name="voxel_size_z" value="$(var downsample_voxel_size)"/>
+        <extra_arg name="use_intra_process_comms" value="true"/>
+      </composable_node>
+    </load_composable_node>
+  </group>
+
   <!-- Perception module -->
   <group>
     <push-ros-namespace namespace="perception"/>
@@ -120,7 +139,7 @@
         <arg name="use_multithread" value="true"/>
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
-        <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+        <arg name="input/pointcloud" value="$(var perception_pointcloud)"/>
       </include>
     </group>
 
@@ -129,7 +148,7 @@
       <push-ros-namespace namespace="occupancy_grid_map"/>
       <include file="$(find-pkg-share tier4_perception_launch)/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml">
         <arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/single_frame/pointcloud_raw"/>
-        <arg name="input/raw_pointcloud" value="$(var input/pointcloud)"/>
+        <arg name="input/raw_pointcloud" value="$(var perception_pointcloud)"/>
         <arg name="output" value="/perception/occupancy_grid_map/map"/>
         <arg name="use_intra_process" value="true"/>
         <arg name="use_multithread" value="true"/>
@@ -150,7 +169,7 @@
         <push-ros-namespace namespace="detection"/>
         <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detection.launch.xml">
           <arg name="mode" value="$(var mode)"/>
-          <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+          <arg name="input/pointcloud" value="$(var perception_pointcloud)"/>
           <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
           <arg name="image_raw0" value="$(var image_raw0)"/>
           <arg name="camera_info0" value="$(var camera_info0)"/>

perception/probabilistic_occupancy_grid_map/README.md

@@ -70,3 +70,39 @@ Additional argument is shown below:
 | `container_name`                    | `occupancy_grid_map_container` |                                                                                               |
 | `input_obstacle_pointcloud`         | `false`                        | only for laserscan based method. If true, the node subscribe obstacle pointcloud              |
 | `input_obstacle_and_raw_pointcloud` | `true`                         | only for laserscan based method. If true, the node subscribe both obstacle and raw pointcloud |
+
+### Downsample input pointcloud(Optional)
+
+If you set `downsample_input_pointcloud` to `true`, the input pointcloud will be downsampled and following topics are also used. This feature is currently only for the pointcloud based occupancy grid map.
+
+- pointcloud_based_occupancy_grid_map method
+
+```yaml
+# downsampled raw and obstacle pointcloud
+/perception/occupancy_grid_map/obstacle/downsample/pointcloud
+/perception/occupancy_grid_map/raw/downsample/pointcloud
+```
+
+- multi_lidar_pointcloud_based_point_cloud
+
+```yaml
+# downsampled raw and obstacle pointcloud
+/perception/occupancy_grid_map/obstacle/downsample/pointcloud
+/perception/occupancy_grid_map/<sensor_name>/raw/downsample/pointcloud
+```
+
+### Test
+
+This package provides unit tests using `gtest`.
+You can run the test by the following command.
+
+```bash
+colcon test --packages-select probabilistic_occupancy_grid_map --event-handlers console_direct+
+```
+
+Test contains the following.
+
+- Unit test for cost value conversion function
+- Unit test for utility functions
+- Unit test for occupancy grid map fusion functions
+- Input/Output test for pointcloud based occupancy grid map

perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml

@@ -8,6 +8,10 @@
       min_height: -1.0
       max_height: 2.0
 
+    # downsample input pointcloud
+    downsample_input_pointcloud: true
+    downsample_voxel_size: 0.25 # [m]
+
     enable_single_frame_mode: false
     # use sensor pointcloud to filter obstacle pointcloud
     filter_obstacle_pointcloud_by_raw_pointcloud: false

perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py

@@ -26,6 +26,47 @@
 import yaml
 
 
+def get_downsample_filter_node(setting: dict) -> ComposableNode:
+    plugin_str = setting["plugin"]
+    voxel_size = setting["voxel_size"]
+    node_name = setting["node_name"]
+    return ComposableNode(
+        package="pointcloud_preprocessor",
+        plugin=plugin_str,
+        name=node_name,
+        remappings=[
+            ("input", setting["input_topic"]),
+            ("output", setting["output_topic"]),
+        ],
+        parameters=[
+            {
+                "voxel_size_x": voxel_size,
+                "voxel_size_y": voxel_size,
+                "voxel_size_z": voxel_size,
+            }
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
+
+def get_downsample_preprocess_nodes(voxel_size: float) -> list:
+    raw_settings = {
+        "plugin": "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent",
+        "node_name": "raw_pc_downsample_filter",
+        "input_topic": LaunchConfiguration("input/raw_pointcloud"),
+        "output_topic": "raw/downsample/pointcloud",
+        "voxel_size": voxel_size,
+    }
+    obstacle_settings = {
+        "plugin": "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent",
+        "node_name": "obstacle_pc_downsample_filter",
+        "input_topic": LaunchConfiguration("input/obstacle_pointcloud"),
+        "output_topic": "obstacle/downsample/pointcloud",
+        "voxel_size": voxel_size,
+    }
+    return [get_downsample_filter_node(raw_settings), get_downsample_filter_node(obstacle_settings)]
+
+
 def launch_setup(context, *args, **kwargs):
     # load parameter files
     param_file = LaunchConfiguration("param_file").perform(context)
@@ -38,19 +79,37 @@ def launch_setup(context, *args, **kwargs):
     with open(updater_param_file, "r") as f:
         occupancy_grid_map_updater_params = yaml.safe_load(f)["/**"]["ros__parameters"]
 
-    composable_nodes = [
+    # composable nodes
+    composable_nodes = []
+
+    # add downsample process
+    downsample_input_pointcloud: bool = pointcloud_based_occupancy_grid_map_node_params[
+        "downsample_input_pointcloud"
+    ]
+    if downsample_input_pointcloud:
+        voxel_grid_size: float = pointcloud_based_occupancy_grid_map_node_params[
+            "downsample_voxel_size"
+        ]
+        downsample_preprocess_nodes = get_downsample_preprocess_nodes(voxel_grid_size)
+        composable_nodes.extend(downsample_preprocess_nodes)
+
+    composable_nodes.append(
         ComposableNode(
             package="probabilistic_occupancy_grid_map",
             plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode",
             name="occupancy_grid_map_node",
             remappings=[
                 (
                     "~/input/obstacle_pointcloud",
-                    LaunchConfiguration("input/obstacle_pointcloud"),
+                    LaunchConfiguration("input/obstacle_pointcloud")
+                    if not downsample_input_pointcloud
+                    else "obstacle/downsample/pointcloud",
                 ),
                 (
                     "~/input/raw_pointcloud",
-                    LaunchConfiguration("input/raw_pointcloud"),
+                    LaunchConfiguration("input/raw_pointcloud")
+                    if not downsample_input_pointcloud
+                    else "raw/downsample/pointcloud",
                 ),
                 ("~/output/occupancy_grid_map", LaunchConfiguration("output")),
             ],
@@ -60,8 +119,8 @@ def launch_setup(context, *args, **kwargs):
                 {"updater_type": LaunchConfiguration("updater_type")},
             ],
             extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
-        ),
-    ]
+        )
+    )
 
     occupancy_grid_map_container = ComposableNodeContainer(
         name=LaunchConfiguration("container_name"),

perception/probabilistic_occupancy_grid_map/package.xml

@@ -34,6 +34,7 @@
   <depend>tier4_autoware_utils</depend>
   <depend>visualization_msgs</depend>
 
+  <exec_depend>pointcloud_preprocessor</exec_depend>
   <exec_depend>pointcloud_to_laserscan</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
@@ -44,6 +45,7 @@
   <test_depend>launch_testing</test_depend>
   <test_depend>launch_testing_ament_cmake</test_depend>
   <test_depend>launch_testing_ros</test_depend>
+  <test_depend>pointcloud_preprocessor</test_depend>
   <export>
     <build_type>ament_cmake</build_type>
   </export>

perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md

@@ -77,18 +77,34 @@ $$
 | ----------------------------- | ------------------------- | ------------------ |
 | `~/output/occupancy_grid_map` | `nav_msgs::OccupancyGrid` | occupancy grid map |
 
+### Related topics
+
+If you set `downsample_input_pointcloud` to `true`, the input pointcloud will be downsampled and following topics are also used.
+
+- pointcloud_based_occupancy_grid_map method
+
+```yaml
+# downsampled raw and obstacle pointcloud
+/perception/occupancy_grid_map/obstacle/downsample/pointcloud
+/perception/occupancy_grid_map/raw/downsample/pointcloud
+```
+
 ## Parameters
 
 ### Node Parameters
 
-| Name                | Type   | Description                                                                                                                      |
-| ------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------- |
-| `map_frame`         | string | map frame                                                                                                                        |
-| `base_link_frame`   | string | base_link frame                                                                                                                  |
-| `use_height_filter` | bool   | whether to height filter for `~/input/obstacle_pointcloud` and `~/input/raw_pointcloud`? By default, the height is set to -1~2m. |
-| `map_length`        | double | The length of the map. -100 if it is 50~50[m]                                                                                    |
-| `map_resolution`    | double | The map cell resolution [m]                                                                                                      |
-| `grid_map_type`     | string | The type of grid map for estimating `UNKNOWN` region behind obstacle point clouds                                                |
+| Name                          | Type   | Description                                                                                                                      |
+| ----------------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------- |
+| `map_frame`                   | string | map frame                                                                                                                        |
+| `base_link_frame`             | string | base_link frame                                                                                                                  |
+| `use_height_filter`           | bool   | whether to height filter for `~/input/obstacle_pointcloud` and `~/input/raw_pointcloud`? By default, the height is set to -1~2m. |
+| `map_length`                  | double | The length of the map. -100 if it is 50~50[m]                                                                                    |
+| `map_resolution`              | double | The map cell resolution [m]                                                                                                      |
+| `grid_map_type`               | string | The type of grid map for estimating `UNKNOWN` region behind obstacle point clouds                                                |
+| `scan_origin`                 | string | The origin of the scan. It should be a sensor frame.                                                                             |
+| `pub_debug_grid`              | bool   | Whether to publish debug grid maps                                                                                               |
+| `downsample_input_pointcloud` | bool   | Whether to downsample the input pointclouds. The downsampled pointclouds are used for the ray tracing.                           |
+| `downsample_voxel_size`       | double | The voxel size for the downsampled pointclouds.                                                                                  |
 
 ## Assumptions / Known limits
 

sensing/pointcloud_preprocessor/CMakeLists.txt

@@ -65,6 +65,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
   src/downsample_filter/voxel_grid_downsample_filter_nodelet.cpp
   src/downsample_filter/random_downsample_filter_nodelet.cpp
   src/downsample_filter/approximate_downsample_filter_nodelet.cpp
+  src/downsample_filter/pickup_based_voxel_grid_downsample_filter.cpp
   src/outlier_filter/ring_outlier_filter_nodelet.cpp
   src/outlier_filter/voxel_grid_outlier_filter_nodelet.cpp
   src/outlier_filter/radius_search_2d_outlier_filter_nodelet.cpp
@@ -114,6 +115,11 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "pointcloud_preprocessor::VoxelGridDownsampleFilterComponent"
   EXECUTABLE voxel_grid_downsample_filter_node)
 
+# -- Pickup Based Voxel Grid Downsample Filter --
+rclcpp_components_register_node(pointcloud_preprocessor_filter
+  PLUGIN "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent"
+  EXECUTABLE pickup_based_voxel_grid_downsample_filter_node)
+
 # -- Random Downsample Filter --
 rclcpp_components_register_node(pointcloud_preprocessor_filter
   PLUGIN "pointcloud_preprocessor::RandomDownsampleFilterComponent"
@@ -214,7 +220,6 @@ ament_auto_package(INSTALL_TO_SHARE
   config
 )
 
-
 if(BUILD_TESTING)
   add_ros_test(
     test/test_distortion_corrector.py

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
 

sensing/pointcloud_preprocessor/docs/downsample-filter.md

@@ -18,6 +18,10 @@ The `downsample_filter` is a node that reduces the number of points.
 
 `pcl::VoxelGrid` is used, which points in each voxel are approximated with their centroid.
 
+### Pickup Based Voxel Grid Downsample Filter
+
+This algorithm samples a single actual point existing within the voxel, not the centroid. The computation cost is low compared to Centroid Based Voxel Grid Filter.
+
 ## Inputs / Outputs
 
 These implementations inherit `pointcloud_preprocessor::Filter` class, please refer [README](../README.md).
@@ -52,8 +56,20 @@ These implementations inherit `pointcloud_preprocessor::Filter` class, please re
 | `voxel_size_y` | double | 0.3           | voxel size y [m] |
 | `voxel_size_z` | double | 0.1           | voxel size z [m] |
 
+### Pickup Based Voxel Grid Downsample Filter
+
+| Name           | Type   | Default Value | Description      |
+| -------------- | ------ | ------------- | ---------------- |
+| `voxel_size_x` | double | 1.0           | voxel size x [m] |
+| `voxel_size_y` | double | 1.0           | voxel size y [m] |
+| `voxel_size_z` | double | 1.0           | voxel size z [m] |
+
 ## Assumptions / Known limits
 
+<!-- cspell: ignore martinus -->
+
+This implementation uses the `robin_hood.h` hashing library by martinus, available under the MIT License at [martinus/robin-hood-hashing](https://github.com/martinus/robin-hood-hashing) on GitHub. Special thanks to martinus for this contribution.
+
 ## (Optional) Error detection and handling
 
 ## (Optional) Performance characterization

sensing/pointcloud_preprocessor/docs/image/pointcloud_preprocess_pipeline.drawio.png

@@ -126,6 +126,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   double timeout_sec_ = 0.1;
 
   bool publish_synchronized_pointcloud_;
+  bool keep_input_frame_in_synchronized_pointcloud_;
+  std::string synchronized_pointcloud_postfix_;
 
   std::set<std::string> not_subscribed_topic_names_;
 
@@ -179,6 +181,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node
   void timer_callback();
 
   void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  std::string replaceSyncTopicNamePostfix(
+    const std::string & original_topic_name, const std::string & postfix);
 
   /** \brief processing time publisher. **/
   std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;