fix(euclidean_cluster): add disuse downsample in clustering pipeline (#4385)

* fix: add unuse downsample launch option

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix: add default param for downsample option

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

* fix typo

Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp>

---------

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
Signed-off-by: Shunsuke Miura <shunsuke.miura@tier4.jp>
Co-authored-by: Shunsuke Miura <shunsuke.miura@tier4.jp>

Author: badai-nguyen

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml

@@ -30,6 +30,7 @@
   <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
   <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_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"/>
@@ -73,6 +74,7 @@
         <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)"/>
       </include>
     </group>
 

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml

@@ -4,6 +4,7 @@
   <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_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"/>
@@ -59,6 +60,7 @@
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="score_threshold" value="$(var score_threshold)"/>
+      <arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
     </include>
   </group>
 

launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml

@@ -5,6 +5,7 @@
   <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"/>
@@ -33,6 +34,7 @@
         <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)"/>
       </include>
     </group>
 

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -58,6 +58,7 @@
   <arg name="image_number" default="6" description="choose image raw number(1-8)"/>
   <arg name="use_vector_map" default="true" description="use vector map in prediction"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+  <arg name="use_downsample_pointcloud" default="false" description="use downsample pointcloud in perception"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg
     name="use_empty_dynamic_object_publisher"
@@ -145,6 +146,7 @@
           <arg name="voxel_grid_param_path" value="$(var object_recognition_detection_voxel_grid_based_euclidean_param_path)"/>
           <arg name="voxel_grid_based_euclidean_param_path" value="$(var object_recognition_detection_voxel_grid_based_euclidean_cluster_param_path)"/>
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+          <arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
           <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 pointcloud_container_name)"/>

perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py

@@ -16,9 +16,10 @@
 from launch.actions import DeclareLaunchArgument
 from launch.actions import OpaqueFunction
 from launch.conditions import IfCondition
-from launch.conditions import UnlessCondition
+from launch.substitutions import AndSubstitution
 from launch.substitutions import AnonName
 from launch.substitutions import LaunchConfiguration
+from launch.substitutions import NotSubstitution
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
@@ -164,7 +165,7 @@ def load_composable_node_param(param_path):
     )
 
     # set euclidean cluster as a component
-    euclidean_cluster_component = ComposableNode(
+    use_downsample_euclidean_cluster_component = ComposableNode(
         package=pkg,
         namespace=ns,
         plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode",
@@ -176,40 +177,108 @@ def load_composable_node_param(param_path):
         parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")],
     )
 
+    use_map_disuse_downsample_euclidean_component = ComposableNode(
+        package=pkg,
+        namespace=ns,
+        plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode",
+        name="euclidean_cluster",
+        remappings=[
+            ("input", "map_filter/pointcloud"),
+            ("output", LaunchConfiguration("output_clusters")),
+        ],
+        parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")],
+    )
+    disuse_map_disuse_downsample_euclidean_component = ComposableNode(
+        package=pkg,
+        namespace=ns,
+        plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode",
+        name="euclidean_cluster",
+        remappings=[
+            ("input", LaunchConfiguration("input_pointcloud")),
+            ("output", LaunchConfiguration("output_clusters")),
+        ],
+        parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")],
+    )
+
     container = ComposableNodeContainer(
         name="euclidean_cluster_container",
         package="rclcpp_components",
         namespace=ns,
         executable="component_container",
-        composable_node_descriptions=[
-            voxel_grid_filter_component,
-            outlier_filter_component,
-            downsample_concat_component,
-            euclidean_cluster_component,
-        ],
+        composable_node_descriptions=[],
         output="screen",
     )
 
-    use_map_loader = LoadComposableNodes(
+    use_map_use_downsample_loader = LoadComposableNodes(
         composable_node_descriptions=[
             compare_map_filter_component,
             use_map_short_range_crop_box_filter_component,
             use_map_long_range_crop_box_filter_component,
+            voxel_grid_filter_component,
+            outlier_filter_component,
+            downsample_concat_component,
+            use_downsample_euclidean_cluster_component,
         ],
         target_container=container,
-        condition=IfCondition(LaunchConfiguration("use_pointcloud_map")),
+        condition=IfCondition(
+            AndSubstitution(
+                LaunchConfiguration("use_pointcloud_map"),
+                LaunchConfiguration("use_downsample_pointcloud"),
+            )
+        ),
     )
 
-    disuse_map_loader = LoadComposableNodes(
+    disuse_map_use_downsample_loader = LoadComposableNodes(
         composable_node_descriptions=[
             disuse_map_short_range_crop_box_filter_component,
             disuse_map_long_range_crop_box_filter_component,
+            voxel_grid_filter_component,
+            outlier_filter_component,
+            downsample_concat_component,
+            use_downsample_euclidean_cluster_component,
         ],
         target_container=container,
-        condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")),
+        condition=IfCondition(
+            AndSubstitution(
+                NotSubstitution(LaunchConfiguration("use_pointcloud_map")),
+                LaunchConfiguration("use_downsample_pointcloud"),
+            )
+        ),
     )
 
-    return [container, use_map_loader, disuse_map_loader]
+    use_map_disuse_downsample_loader = LoadComposableNodes(
+        composable_node_descriptions=[
+            compare_map_filter_component,
+            use_map_disuse_downsample_euclidean_component,
+        ],
+        target_container=container,
+        condition=IfCondition(
+            AndSubstitution(
+                (LaunchConfiguration("use_pointcloud_map")),
+                NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")),
+            )
+        ),
+    )
+
+    disuse_map_disuse_downsample_loader = LoadComposableNodes(
+        composable_node_descriptions=[
+            disuse_map_disuse_downsample_euclidean_component,
+        ],
+        target_container=container,
+        condition=IfCondition(
+            AndSubstitution(
+                NotSubstitution(LaunchConfiguration("use_pointcloud_map")),
+                NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")),
+            )
+        ),
+    )
+    return [
+        container,
+        use_map_use_downsample_loader,
+        disuse_map_use_downsample_loader,
+        use_map_disuse_downsample_loader,
+        disuse_map_disuse_downsample_loader,
+    ]
 
 
 def generate_launch_description():
@@ -222,6 +291,7 @@ def add_launch_arg(name: str, default_value=None):
             add_launch_arg("input_map", "/map/pointcloud_map"),
             add_launch_arg("output_clusters", "clusters"),
             add_launch_arg("use_pointcloud_map", "false"),
+            add_launch_arg("use_downsample_pointcloud", "false"),
             add_launch_arg(
                 "voxel_grid_param_path",
                 [

perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml

@@ -4,6 +4,7 @@
   <arg name="input_map" default="/map/pointcloud_map"/>
   <arg name="output_clusters" default="clusters"/>
   <arg name="use_pointcloud_map" default="false"/>
+  <arg name="use_downsample_pointcloud" default="false"/>
   <arg name="voxel_grid_param_path" default="$(find-pkg-share euclidean_cluster)/config/voxel_grid.param.yaml"/>
   <arg name="compare_map_param_path" default="$(find-pkg-share euclidean_cluster)/config/compare_map.param.yaml"/>
   <arg name="outlier_param_path" default="$(find-pkg-share euclidean_cluster)/config/outlier.param.yaml"/>
@@ -14,6 +15,7 @@
     <arg name="input_map" value="$(var input_map)"/>
     <arg name="output_clusters" value="$(var output_clusters)"/>
     <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+    <arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
     <arg name="voxel_grid_param_path" value="$(var voxel_grid_param_path)"/>
     <arg name="compare_map_param_path" value="$(var compare_map_param_path)"/>
     <arg name="outlier_param_path" value="$(var outlier_param_path)"/>