From fe1358f257f9710a52a2556c5e283509231126cd Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 25 Sep 2023 19:01:01 +0900 Subject: [PATCH 1/2] fix: add downsample_after concat Signed-off-by: badai-nguyen --- .../launch/pointcloud_preprocessor.launch.py | 23 +++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/aip_x2_launch/launch/pointcloud_preprocessor.launch.py b/aip_x2_launch/launch/pointcloud_preprocessor.launch.py index cca7ebc5..be14744f 100644 --- 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", From 4554351baac0473d66911d3a26dbba197826b275 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 28 Sep 2023 23:52:27 +0900 Subject: [PATCH 2/2] fix: add downsample_after concat for xx1 Signed-off-by: badai-nguyen --- .../launch/pointcloud_preprocessor.launch.py | 23 +++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py b/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py index 388a0c98..62ea0756 100644 --- a/aip_xx1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_xx1_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=[ { @@ -56,6 +56,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"), @@ -75,7 +93,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")), ) @@ -94,6 +112,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "False") 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",