fix: add downsample_after concat

Signed-off-by: badai-nguyen <[email protected]>
tier4 · Sep 25, 2023 · 6219a82 · 6219a82
1 parent 0245cd8
commit 6219a82
Showing 1 changed file with 21 additions and 2 deletions.
diff --git a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py
@@ -33,7 +33,7 @@ def launch_setup(context, *args, **kwargs):
         name="concatenate_data",
         remappings=[
             ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"),
-            ("output", "concatenated/pointcloud"),
+            ("output", "concatenated/pointcloud_raw"),
         ],
         parameters=[
             {
@@ -55,6 +55,24 @@ def launch_setup(context, *args, **kwargs):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    concat_downsample = ComposableNode(
+        package="pointcloud_preprocessor",
+        plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent",
+        name="voxel_grid_downsample_filter",
+        remappings=[
+            ("input", "concatenated/pointcloud_raw"),
+            ("output", "concatenated/pointcloud"),
+        ],
+        parameters=[
+            {
+                "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")}],
+    )
+
     # set container to run all required components in the same process
     container = ComposableNodeContainer(
         name=LaunchConfiguration("container_name"),
@@ -74,7 +92,7 @@ def launch_setup(context, *args, **kwargs):
 
     # load concat or passthrough filter
     concat_loader = LoadComposableNodes(
-        composable_node_descriptions=[concat_component],
+        composable_node_descriptions=[concat_component, concat_downsample],
         target_container=target_container,
         condition=IfCondition(LaunchConfiguration("use_concat_filter")),
     )
@@ -93,6 +111,7 @@ def add_launch_arg(name: str, default_value=None):
     add_launch_arg("use_intra_process", "True")
     add_launch_arg("use_pointcloud_container", "False")
     add_launch_arg("container_name", "pointcloud_preprocessor_container")
+    add_launch_arg("voxel_size", 0.1),
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",