refactor(perception): rearrange clustering pipeline (autowarefoundati…

…on#4999) * fix: change downsample filter Signed-off-by: badai-nguyen <[email protected]> * fix: remove downsamle after compare map Signed-off-by: badai-nguyen <[email protected]> * fix: add low range cropbox Signed-off-by: badai-nguyen <[email protected]> * refactor: use_pointcloud_map Signed-off-by: badai-nguyen <[email protected]> * chore: refactor Signed-off-by: badai-nguyen <[email protected]> * fix: add roi based clustering option Signed-off-by: badai-nguyen <[email protected]> * chore: change node name Signed-off-by: badai-nguyen <[email protected]> * fix: launch argument pasrer Signed-off-by: badai-nguyen <[email protected]> --------- Signed-off-by: badai-nguyen <[email protected]>
TaikiYamada4 · Sep 19, 2023 · 8eeb11d · 8eeb11d
1 parent 498dbef
commit 8eeb11d
Show file tree

Hide file tree

Showing 15 changed files with 103 additions and 393 deletions.
diff --git a/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -11,13 +11,12 @@
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="score_threshold" default="0.35"/>
 
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
-  <arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
   <arg name="remove_unknown" default="true"/>
   <arg name="trust_distance" default="30.0"/>
+  <arg name="use_roi_based_cluster" default="false"/>
 
   <!-- Camera param -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
@@ -59,35 +58,35 @@
     <arg name="image_number" value="$(var image_number)"/>
   </include> -->
 
-  <!-- Pointcloud map filter -->
+  <!-- Pointcloud filter -->
   <group>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py" if="$(var use_pointcloud_map)">
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py">
       <arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
       <arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
       <arg name="use_intra_process" value="true"/>
       <arg name="use_multithread" value="true"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
     </include>
   </group>
 
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
     <group>
-      <let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
-      <let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
+      <let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
+      <let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-        <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
-        <arg name="output_clusters" value="euclidean_cluster/clusters"/>
-        <arg name="use_pointcloud_map" value="false"/>
-        <arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
+        <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
+        <arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
+        <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       </include>
     </group>
 
     <!-- roi based clustering -->
     <group>
-      <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml">
+      <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml" if="$(var use_roi_based_cluster)">
         <arg name="input/camera_info0" value="$(var camera_info0)"/>
         <arg name="input/rois0" value="$(var detection_rois0)"/>
         <arg name="input/camera_info1" value="$(var camera_info1)"/>
@@ -120,7 +119,7 @@
 
     <!-- simple_cluster_merger -->
     <group>
-      <include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml">
+      <include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml" if="$(var use_roi_based_cluster)">
         <arg name="input/cluster0" value="euclidean_cluster/clusters"/>
         <arg name="input/cluster1" value="roi_cluster/clusters"/>
         <arg name="output/clusters" value="clusters"/>

diff --git a/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
@@ -11,8 +11,6 @@
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="score_threshold" default="0.35"/>
 
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
-  <arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
@@ -78,29 +76,29 @@
     </include>
   </group>
 
-  <!-- Pointcloud map filter -->
+  <!-- Pointcloud filter -->
   <group>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py" if="$(var use_pointcloud_map)">
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py">
       <arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
       <arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
       <arg name="use_intra_process" value="true"/>
       <arg name="use_multithread" value="true"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
     </include>
   </group>
 
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
     <group>
-      <let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
-      <let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
+      <let name="euclidean_cluster_output" value="euclidean_cluster/clusters" if="$(var use_roi_based_cluster)"/>
+      <let name="euclidean_cluster_output" value="clusters" unless="$(var use_roi_based_cluster)"/>
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-        <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
-        <arg name="output_clusters" value="clusters"/>
-        <arg name="use_pointcloud_map" value="false"/>
-        <arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
+        <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
+        <arg name="output_clusters" value="$(var euclidean_cluster_output)"/>
+        <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       </include>
     </group>
 
@@ -151,7 +149,7 @@
 
       <!-- roi based clustering -->
       <group>
-        <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml">
+        <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml" if="$(var use_roi_based_cluster)">
           <arg name="input/camera_info0" value="$(var camera_info0)"/>
           <arg name="input/rois0" value="$(var detection_rois0)"/>
           <arg name="input/camera_info1" value="$(var camera_info1)"/>
@@ -184,7 +182,7 @@
 
       <!-- simple_cluster_merger -->
       <group>
-        <include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml">
+        <include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml" if="$(var use_roi_based_cluster)">
           <arg name="input/cluster0" value="euclidean_cluster/clusters"/>
           <arg name="input/cluster1" value="roi_cluster/clusters"/>
           <arg name="output/clusters" value="clusters"/>

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
@@ -5,7 +5,6 @@
   <!-- lidar param -->
   <arg name="input/pointcloud"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
@@ -65,6 +64,8 @@
       <arg name="remove_unknown" value="$(var remove_unknown)"/>
       <arg name="trust_distance" value="$(var trust_distance)"/>
       <arg name="input/radar" value="$(var input/radar)"/>
+      <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
+      <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
     </include>
   </group>
 
@@ -94,6 +95,8 @@
       <arg name="use_object_filter" value="$(var use_object_filter)"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
+      <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
+      <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
     </include>
   </group>
 
@@ -107,6 +110,7 @@
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="input/radar" value="$(var input/radar)"/>
+      <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
     </include>
   </group>
 
@@ -119,6 +123,7 @@
       <arg name="use_object_filter" value="$(var use_object_filter)"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
+      <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
     </include>
   </group>
 

diff --git a/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -4,8 +4,6 @@
   <arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
   <arg name="output/objects" default="objects"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `clustering`"/>
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
-  <arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
@@ -19,29 +17,27 @@
   <arg name="object_recognition_detection_object_merger_data_association_matrix_param_path" default="$(find-pkg-share object_merger)/config/data_association_matrix.param.yaml"/>
   <arg name="object_recognition_detection_object_merger_distance_threshold_list_path" default="$(find-pkg-share object_merger)/config/overlapped_judge.param.yaml"/>
 
-  <!-- Pointcloud map filter -->
+  <!-- Pointcloud filter -->
   <group>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py" if="$(var use_pointcloud_map)">
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/pointcloud_map_filter.launch.py">
       <arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
       <arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
       <arg name="use_intra_process" value="true"/>
       <arg name="use_multithread" value="true"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
     </include>
   </group>
 
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
-    <let name="clustering/input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud" if="$(var use_pointcloud_map)"/>
-    <let name="clustering/input/pointcloud" value="$(var input/obstacle_segmentation/pointcloud)" unless="$(var use_pointcloud_map)"/>
     <group>
       <include file="$(find-pkg-share euclidean_cluster)/launch/voxel_grid_based_euclidean_cluster.launch.xml">
-        <arg name="input_pointcloud" value="$(var clustering/input/pointcloud)"/>
+        <arg name="input_pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
         <arg name="output_clusters" value="clusters"/>
-        <arg name="use_pointcloud_map" value="false"/>
-        <arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
+        <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       </include>
     </group>
 

diff --git a/...ception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/...ception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
@@ -3,7 +3,6 @@
   <!-- lidar param -->
   <arg name="input/pointcloud"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `clustering`"/>
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
@@ -19,6 +18,7 @@
       <arg name="output/objects" value="lidar/objects"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
       <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+      <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       <arg name="use_object_filter" value="$(var use_object_filter)"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>

diff --git a/...er4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/...er4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py
@@ -35,7 +35,6 @@ def __init__(self, context):
         )
         with open(pointcloud_map_filter_param_path, "r") as f:
             self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"]
-        self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"]
         self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"]
         self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"]
         self.downsize_ratio_z_axis = self.pointcloud_map_filter_param["downsize_ratio_z_axis"]
@@ -46,55 +45,48 @@ def __init__(self, context):
         ]
         self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"]
         self.publish_debug_pcd = self.pointcloud_map_filter_param["publish_debug_pcd"]
+        self.use_pointcloud_map = LaunchConfiguration("use_pointcloud_map").perform(context)
 
     def create_pipeline(self):
-        if self.use_down_sample_filter:
-            return self.create_down_sample_pipeline()
+        if self.use_pointcloud_map == "true":
+            return self.create_compare_map_pipeline()
         else:
-            return self.create_normal_pipeline()
+            return self.create_no_compare_map_pipeline()
 
-    def create_normal_pipeline(self):
+    def create_no_compare_map_pipeline(self):
         components = []
         components.append(
             ComposableNode(
-                package="compare_map_segmentation",
-                plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent",
-                name="voxel_based_compare_map_filter",
+                package="pointcloud_preprocessor",
+                plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
+                name="voxel_grid_downsample_filter",
                 remappings=[
                     ("input", LaunchConfiguration("input_topic")),
-                    ("map", "/map/pointcloud_map"),
                     ("output", LaunchConfiguration("output_topic")),
-                    ("map_loader_service", "/map/get_differential_pointcloud_map"),
-                    ("kinematic_state", "/localization/kinematic_state"),
                 ],
                 parameters=[
                     {
-                        "distance_threshold": self.distance_threshold,
-                        "downsize_ratio_z_axis": self.downsize_ratio_z_axis,
-                        "timer_interval_ms": self.timer_interval_ms,
-                        "use_dynamic_map_loading": self.use_dynamic_map_loading,
-                        "map_update_distance_threshold": self.map_update_distance_threshold,
-                        "map_loader_radius": self.map_loader_radius,
-                        "publish_debug_pcd": self.publish_debug_pcd,
-                        "input_frame": "map",
+                        "voxel_size_x": self.voxel_size,
+                        "voxel_size_y": self.voxel_size,
+                        "voxel_size_z": self.voxel_size,
                     }
                 ],
                 extra_arguments=[
-                    {"use_intra_process_comms": False},
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
                 ],
-            )
+            ),
         )
         return components
 
-    def create_down_sample_pipeline(self):
+    def create_compare_map_pipeline(self):
         components = []
         down_sample_topic = (
             "/perception/obstacle_segmentation/pointcloud_map_filtered/downsampled/pointcloud"
         )
         components.append(
             ComposableNode(
                 package="pointcloud_preprocessor",
-                plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
+                plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
                 name="voxel_grid_downsample_filter",
                 remappings=[
                     ("input", LaunchConfiguration("input_topic")),
@@ -177,6 +169,7 @@ def add_launch_arg(name: str, default_value=None):
     add_launch_arg("use_intra_process", "True")
     add_launch_arg("use_pointcloud_container", "False")
     add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container")
+    add_launch_arg("use_pointcloud_map", "true")
     set_container_executable = SetLaunchConfiguration(
         "container_executable",
         "component_container",