fix(euclidean_cluster): keep pointcloud resolution in long-range for …

…Euclidean cluster (#2749) * fix: change downnsample split pipeline Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: add downsample range params Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: revise disuse map_filter case Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
autowarefoundation · Jan 27, 2023 · 1f62b91 · 1f62b91
1 parent 0717cd4
commit 1f62b91
Show file tree

Hide file tree

Showing 2 changed files with 110 additions and 14 deletions.
diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml
@@ -6,3 +6,9 @@
     min_cluster_size: 10
     max_cluster_size: 3000
     use_height: false
+    max_x: 70.0
+    min_x: -70.0
+    max_y: 70.0
+    min_y: -70.0
+    max_z: 4.5
+    min_z: -4.5
diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py
@@ -49,23 +49,85 @@ def load_composable_node_param(param_path):
         parameters=[load_composable_node_param("compare_map_param_path")],
     )
 
-    # set voxel grid filter as a component
-    use_map_voxel_grid_filter_component = ComposableNode(
+    # separate range of poincloud when map_filter used
+    use_map_short_range_crop_box_filter_component = ComposableNode(
         package="pointcloud_preprocessor",
         namespace=ns,
-        plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
-        name=AnonName("voxel_grid_filter"),
-        remappings=[("input", "map_filter/pointcloud"), ("output", "downsampled/pointcloud")],
-        parameters=[load_composable_node_param("voxel_grid_param_path")],
+        plugin="pointcloud_preprocessor::CropBoxFilterComponent",
+        name="short_distance_crop_box_range",
+        remappings=[
+            ("input", "map_filter/pointcloud"),
+            ("output", "short_range/pointcloud"),
+        ],
+        parameters=[
+            load_composable_node_param("voxel_grid_based_euclidean_param_path"),
+            {
+                "negative": False,
+            },
+        ],
     )
-    disuse_map_voxel_grid_filter_component = ComposableNode(
+
+    use_map_long_range_crop_box_filter_component = ComposableNode(
+        package="pointcloud_preprocessor",
+        namespace=ns,
+        plugin="pointcloud_preprocessor::CropBoxFilterComponent",
+        name="long_distance_crop_box_range",
+        remappings=[
+            ("input", LaunchConfiguration("input_pointcloud")),
+            ("output", "long_range/pointcloud"),
+        ],
+        parameters=[
+            load_composable_node_param("voxel_grid_based_euclidean_param_path"),
+            {
+                "negative": True,
+            },
+        ],
+    )
+
+    # disuse_map_voxel_grid_filter
+    disuse_map_short_range_crop_box_filter_component = ComposableNode(
+        package="pointcloud_preprocessor",
+        namespace=ns,
+        plugin="pointcloud_preprocessor::CropBoxFilterComponent",
+        name="short_distance_crop_box_range",
+        remappings=[
+            ("input", LaunchConfiguration("input_pointcloud")),
+            ("output", "short_range/pointcloud"),
+        ],
+        parameters=[
+            load_composable_node_param("voxel_grid_based_euclidean_param_path"),
+            {
+                "negative": False,
+            },
+        ],
+    )
+
+    disuse_map_long_range_crop_box_filter_component = ComposableNode(
+        package="pointcloud_preprocessor",
+        namespace=ns,
+        plugin="pointcloud_preprocessor::CropBoxFilterComponent",
+        name="long_distance_crop_box_range",
+        remappings=[
+            ("input", LaunchConfiguration("input_pointcloud")),
+            ("output", "long_range/pointcloud"),
+        ],
+        parameters=[
+            load_composable_node_param("voxel_grid_based_euclidean_param_path"),
+            {
+                "negative": True,
+            },
+        ],
+    )
+
+    # set voxel grid filter as a component
+    voxel_grid_filter_component = ComposableNode(
         package="pointcloud_preprocessor",
         namespace=ns,
         plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent",
         name=AnonName("voxel_grid_filter"),
         remappings=[
-            ("input", LaunchConfiguration("input_pointcloud")),
-            ("output", "downsampled/pointcloud"),
+            ("input", "short_range/pointcloud"),
+            ("output", "downsampled/short_range/pointcloud"),
         ],
         parameters=[load_composable_node_param("voxel_grid_param_path")],
     )
@@ -76,18 +138,37 @@ def load_composable_node_param(param_path):
         namespace=ns,
         plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent",
         name="outlier_filter",
-        remappings=[("input", "downsampled/pointcloud"), ("output", "outlier_filter/pointcloud")],
+        remappings=[
+            ("input", "downsampled/short_range/pointcloud"),
+            ("output", "outlier_filter/pointcloud"),
+        ],
         parameters=[load_composable_node_param("outlier_param_path")],
     )
 
+    # concat with-outlier pointcloud and without-outlier pcl
+    downsample_concat_component = ComposableNode(
+        package="pointcloud_preprocessor",
+        namespace=ns,
+        plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent",
+        name="concat_downsampled_pcl",
+        remappings=[("output", "downsampled/concatenated/pointcloud")],
+        parameters=[
+            {
+                "input_topics": ["long_range/pointcloud", "outlier_filter/pointcloud"],
+                "output_frame": "base_link",
+            }
+        ],
+        extra_arguments=[{"use_intra_process_comms": True}],
+    )
+
     # set euclidean cluster as a component
     euclidean_cluster_component = ComposableNode(
         package=pkg,
         namespace=ns,
         plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode",
         name="euclidean_cluster",
         remappings=[
-            ("input", "outlier_filter/pointcloud"),
+            ("input", "downsampled/concatenated/pointcloud"),
             ("output", LaunchConfiguration("output_clusters")),
         ],
         parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")],
@@ -98,21 +179,30 @@ def load_composable_node_param(param_path):
         package="rclcpp_components",
         namespace=ns,
         executable="component_container",
-        composable_node_descriptions=[outlier_filter_component, euclidean_cluster_component],
+        composable_node_descriptions=[
+            voxel_grid_filter_component,
+            outlier_filter_component,
+            downsample_concat_component,
+            euclidean_cluster_component,
+        ],
         output="screen",
     )
 
     use_map_loader = LoadComposableNodes(
         composable_node_descriptions=[
             compare_map_filter_component,
-            use_map_voxel_grid_filter_component,
+            use_map_short_range_crop_box_filter_component,
+            use_map_long_range_crop_box_filter_component,
         ],
         target_container=container,
         condition=IfCondition(LaunchConfiguration("use_pointcloud_map")),
     )
 
     disuse_map_loader = LoadComposableNodes(
-        composable_node_descriptions=[disuse_map_voxel_grid_filter_component],
+        composable_node_descriptions=[
+            disuse_map_short_range_crop_box_filter_component,
+            disuse_map_long_range_crop_box_filter_component,
+        ],
         target_container=container,
         condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")),
     )