From 63ba4a4291a4cafa3155b1c72ef2b314fed281f1 Mon Sep 17 00:00:00 2001
From: badai-nguyen <dai.nguyen@tier4.jp>
Date: Mon, 25 Sep 2023 22:55:44 +0900
Subject: [PATCH 1/2] fix: move downsample node

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
---
 .../detection/pointcloud_map_filter.launch.py | 26 +------------------
 1 file changed, 1 insertion(+), 25 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py
index 93d395ca3e466..c5be18422a17f 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py
@@ -80,37 +80,13 @@ def create_no_compare_map_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::ApproximateDownsampleFilterComponent",
-                name="voxel_grid_downsample_filter",
-                remappings=[
-                    ("input", LaunchConfiguration("input_topic")),
-                    ("output", down_sample_topic),
-                ],
-                parameters=[
-                    {
-                        "voxel_size_x": self.voxel_size,
-                        "voxel_size_y": self.voxel_size,
-                        "voxel_size_z": self.voxel_size,
-                    }
-                ],
-                extra_arguments=[
-                    {"use_intra_process_comms": LaunchConfiguration("use_intra_process")}
-                ],
-            ),
-        )
         components.append(
             ComposableNode(
                 package="compare_map_segmentation",
                 plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent",
                 name="voxel_based_compare_map_filter",
                 remappings=[
-                    ("input", down_sample_topic),
+                    ("input", LaunchConfiguration("input_topic")),
                     ("map", "/map/pointcloud_map"),
                     ("output", LaunchConfiguration("output_topic")),
                     ("map_loader_service", "/map/get_differential_pointcloud_map"),

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 2/2] 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",