From bc48f9ef5a1c41dd6ff41c3054d640cb698398f3 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Wed, 11 Oct 2023 17:17:19 +0900
Subject: [PATCH] fix: move downsample before common ground-segmentation

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../ground_segmentation.launch.py             | 27 +++++++++++++++++--
 1 file changed, 25 insertions(+), 2 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
index 7ed5860612601..d6e2956a5406c 100644
--- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py
+++ b/launch/tier4_perception_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",