diff --git a/perception_launch/config/object_segmentation/ground_segmentation/elevation_map_parameters.yaml b/perception_launch/config/object_segmentation/ground_segmentation/elevation_map_parameters.yaml new file mode 100644 index 0000000000000..955a9e197ea0b --- /dev/null +++ b/perception_launch/config/object_segmentation/ground_segmentation/elevation_map_parameters.yaml @@ -0,0 +1,30 @@ +pcl_grid_map_extraction: + num_processing_threads: 12 + cloud_transform: + translation: + x: 0.0 + y: 0.0 + z: 0.0 + rotation: #intrinsic rotation X-Y-Z (r-p-y)sequence + r: 0.0 + p: 0.0 + y: 0.0 + cluster_extraction: + cluster_tolerance: 0.2 + min_num_points: 3 + max_num_points: 1000000 + outlier_removal: + is_remove_outliers: false + mean_K: 10 + stddev_threshold: 1.0 + downsampling: + is_downsample_cloud: false + voxel_size: + x: 0.02 + y: 0.02 + z: 0.02 + grid_map: + min_num_points_per_cell: 3 + resolution: 0.3 + height_type: 1 + height_thresh: 1.0 diff --git a/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 465727843822f..9c4b1387ede26 100644 --- a/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -21,9 +21,11 @@ from launch.conditions import IfCondition from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare import yaml @@ -162,15 +164,53 @@ def create_ransac_pipeline(ground_segmentation_param): ] -def create_elevation_map_filter_pipeline(): +def create_elevation_map_filter_pipeline(ground_segmentation_param): + + elevation_map_loader = ComposableNode( + package="elevation_map_loader", + plugin="ElevationMapLoaderNode", + name="elevation_map_loader", + remappings=[ + ("output/elevation_map", "elevation_map"), + ("input/pointcloud_map", "/map/pointcloud_map"), + ("input/vector_map", "/map/vector_map"), + ], + parameters=[ + { + "use_lane_filter": False, + "use_inpaint": True, + "inpaint_radius": 1.0, + "param_file_path": PathJoinSubstitution( + [ + FindPackageShare("perception_launch"), + "config", + "object_segmentation", + "ground_segmentation", + "elevation_map_parameters.yaml", + ] + ), + "elevation_map_directory": PathJoinSubstitution( + [FindPackageShare("elevation_map_loader"), "data", "elevation_maps"] + ), + "use_elevation_map_cloud_publisher": False, + } + ], + extra_arguments=[{"use_intra_process_comms": False}], + ) + compare_elevation_map_filter_component = ComposableNode( package="compare_map_segmentation", plugin="compare_map_segmentation::CompareElevationMapFilterComponent", name="compare_elevation_map_filter", remappings=[ - ("input", "no_ground/oneshot/pointcloud"), + ( + "input", + "no_ground/oneshot/pointcloud" + if bool(ground_segmentation_param["additional_lidars"]) + else "no_ground/pointcloud", + ), ("output", "map_filtered/pointcloud"), - ("input/elevation_map", "/map/elevation_map"), + ("input/elevation_map", "elevation_map"), ], parameters=[ { @@ -224,6 +264,7 @@ def create_elevation_map_filter_pipeline(): ) return [ + elevation_map_loader, compare_elevation_map_filter_component, downsampling_component, voxel_grid_outlier_filter_component, @@ -369,7 +410,9 @@ def launch_setup(context, *args, **kwargs): ) compare_map_component_loader = LoadComposableNodes( - composable_node_descriptions=create_elevation_map_filter_pipeline(), + composable_node_descriptions=create_elevation_map_filter_pipeline( + ground_segmentation_param + ), target_container=target_container, condition=IfCondition( LaunchConfiguration(