From f5fac7ce0446358c840bc47a6bc62abbe7645683 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 27 Jan 2023 14:42:18 +0900 Subject: [PATCH] fix(euclidean_cluster): keep pointcloud resolution in long-range for Euclidean cluster (#2749) * fix: change downnsample split pipeline Signed-off-by: badai-nguyen * fix: add downsample range params Signed-off-by: badai-nguyen * fix: revise disuse map_filter case Signed-off-by: badai-nguyen Signed-off-by: badai-nguyen --- ...el_grid_based_euclidean_cluster.param.yaml | 6 + ...xel_grid_based_euclidean_cluster.launch.py | 118 +++++++++++++++--- 2 files changed, 110 insertions(+), 14 deletions(-) diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml index 27ba5a8178fcf..1ff1e7c8ab55e 100644 --- a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml +++ b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml @@ -6,3 +6,9 @@ min_cluster_size: 10 max_cluster_size: 3000 use_height: false + max_x: 70.0 + min_x: -70.0 + max_y: 70.0 + min_y: -70.0 + max_z: 4.5 + min_z: -4.5 diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 2cfc5142f6fd8..f523245092564 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -49,23 +49,85 @@ def load_composable_node_param(param_path): parameters=[load_composable_node_param("compare_map_param_path")], ) - # set voxel grid filter as a component - use_map_voxel_grid_filter_component = ComposableNode( + # separate range of poincloud when map_filter used + use_map_short_range_crop_box_filter_component = ComposableNode( package="pointcloud_preprocessor", namespace=ns, - plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), - remappings=[("input", "map_filter/pointcloud"), ("output", "downsampled/pointcloud")], - parameters=[load_composable_node_param("voxel_grid_param_path")], + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="short_distance_crop_box_range", + remappings=[ + ("input", "map_filter/pointcloud"), + ("output", "short_range/pointcloud"), + ], + parameters=[ + load_composable_node_param("voxel_grid_based_euclidean_param_path"), + { + "negative": False, + }, + ], ) - disuse_map_voxel_grid_filter_component = ComposableNode( + + use_map_long_range_crop_box_filter_component = ComposableNode( + package="pointcloud_preprocessor", + namespace=ns, + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="long_distance_crop_box_range", + remappings=[ + ("input", LaunchConfiguration("input_pointcloud")), + ("output", "long_range/pointcloud"), + ], + parameters=[ + load_composable_node_param("voxel_grid_based_euclidean_param_path"), + { + "negative": True, + }, + ], + ) + + # disuse_map_voxel_grid_filter + disuse_map_short_range_crop_box_filter_component = ComposableNode( + package="pointcloud_preprocessor", + namespace=ns, + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="short_distance_crop_box_range", + remappings=[ + ("input", LaunchConfiguration("input_pointcloud")), + ("output", "short_range/pointcloud"), + ], + parameters=[ + load_composable_node_param("voxel_grid_based_euclidean_param_path"), + { + "negative": False, + }, + ], + ) + + disuse_map_long_range_crop_box_filter_component = ComposableNode( + package="pointcloud_preprocessor", + namespace=ns, + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="long_distance_crop_box_range", + remappings=[ + ("input", LaunchConfiguration("input_pointcloud")), + ("output", "long_range/pointcloud"), + ], + parameters=[ + load_composable_node_param("voxel_grid_based_euclidean_param_path"), + { + "negative": True, + }, + ], + ) + + # set voxel grid filter as a component + voxel_grid_filter_component = ComposableNode( package="pointcloud_preprocessor", namespace=ns, plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", name=AnonName("voxel_grid_filter"), remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "downsampled/pointcloud"), + ("input", "short_range/pointcloud"), + ("output", "downsampled/short_range/pointcloud"), ], parameters=[load_composable_node_param("voxel_grid_param_path")], ) @@ -76,10 +138,29 @@ def load_composable_node_param(param_path): namespace=ns, plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", name="outlier_filter", - remappings=[("input", "downsampled/pointcloud"), ("output", "outlier_filter/pointcloud")], + remappings=[ + ("input", "downsampled/short_range/pointcloud"), + ("output", "outlier_filter/pointcloud"), + ], parameters=[load_composable_node_param("outlier_param_path")], ) + # concat with-outlier pointcloud and without-outlier pcl + downsample_concat_component = ComposableNode( + package="pointcloud_preprocessor", + namespace=ns, + plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="concat_downsampled_pcl", + remappings=[("output", "downsampled/concatenated/pointcloud")], + parameters=[ + { + "input_topics": ["long_range/pointcloud", "outlier_filter/pointcloud"], + "output_frame": "base_link", + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + # set euclidean cluster as a component euclidean_cluster_component = ComposableNode( package=pkg, @@ -87,7 +168,7 @@ def load_composable_node_param(param_path): plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ - ("input", "outlier_filter/pointcloud"), + ("input", "downsampled/concatenated/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], @@ -98,21 +179,30 @@ def load_composable_node_param(param_path): package="rclcpp_components", namespace=ns, executable="component_container", - composable_node_descriptions=[outlier_filter_component, euclidean_cluster_component], + composable_node_descriptions=[ + voxel_grid_filter_component, + outlier_filter_component, + downsample_concat_component, + euclidean_cluster_component, + ], output="screen", ) use_map_loader = LoadComposableNodes( composable_node_descriptions=[ compare_map_filter_component, - use_map_voxel_grid_filter_component, + use_map_short_range_crop_box_filter_component, + use_map_long_range_crop_box_filter_component, ], target_container=container, condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), ) disuse_map_loader = LoadComposableNodes( - composable_node_descriptions=[disuse_map_voxel_grid_filter_component], + composable_node_descriptions=[ + disuse_map_short_range_crop_box_filter_component, + disuse_map_long_range_crop_box_filter_component, + ], target_container=container, condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), )