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"), 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",