fix: move downsample before common ground-segmentation

Signed-off-by: badai-nguyen <[email protected]>
tier4 · Oct 11, 2023 · bc48f9e · bc48f9e
1 parent 63ba4a4
commit bc48f9e
Showing 1 changed file with 25 additions and 2 deletions.
diff --git a/...ion_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/...ion_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
@@ -225,14 +225,36 @@ def create_common_pipeline(self, input_topic, output_topic):
                 ],
             )
         )
-
+        components.append(
+            ComposableNode(
+                package="pointcloud_preprocessor",
+                plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
+                name="voxel_grid_downsample_filter",
+                remappings=[
+                    ("input", "range_cropped/pointcloud"),
+                    ("output", "range_cropped/downsampled/pointcloud"),
+                ],
+                parameters=[
+                    {
+                        "input_frame": LaunchConfiguration("base_frame"),
+                        "output_frame": LaunchConfiguration("base_frame"),
+                        "voxel_size_x": LaunchConfiguration("voxel_size"),
+                        "voxel_size_y": LaunchConfiguration("voxel_size"),
+                        "voxel_size_z": LaunchConfiguration("voxel_size"),
+                    }
+                ],
+                extra_arguments=[
+                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
+                ],
+            )
+        )
         components.append(
             ComposableNode(
                 package="ground_segmentation",
                 plugin=self.ground_segmentation_param["common_ground_filter"]["plugin"],
                 name="common_ground_filter",
                 remappings=[
-                    ("input", "range_cropped/pointcloud"),
+                    ("input", "range_cropped/downsampled/pointcloud"),
                     ("output", output_topic),
                 ],
                 parameters=[
@@ -521,6 +543,7 @@ def add_launch_arg(name: str, default_value=None):
     add_launch_arg("use_pointcloud_container", "False")
     add_launch_arg("container_name", "perception_pipeline_container")
     add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud")
+    add_launch_arg("voxel_size", "0.1")
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",