From f6bcfd7af39fb68bcc1e93651689f6542ba1d979 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Fri, 5 Apr 2024 01:53:42 +0900 Subject: [PATCH 01/10] chore(ground_segmentation): change parameter name Signed-off-by: 1222-takeshi --- .../ground_segmentation.param.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index 2795ceea..0f3ea457 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -9,12 +9,12 @@ common_crop_box_filter: parameters: - min_x: -50.0 - max_x: 100.0 - min_y: -50.0 - max_y: 50.0 - max_z: 2.5 - min_z: -0.5 + min_x: -100.0 + max_x: 150.0 + min_y: -70.0 + max_y: 70.0 + margin_max_z: 0.0 # to extend the crop box max_z from vehicle_height + margin_min_z: -2.5 # to extend the crop box min_z from ground negative: False common_ground_filter: From 1aa106dd889f6530f9ec3a538c385f49b403286d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 4 Apr 2024 17:01:33 +0000 Subject: [PATCH 02/10] ci(pre-commit): autofix --- aip_x1_1_launch/launch/pandar_node_container.launch.py | 2 ++ aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py | 2 +- common_sensor_launch/package.xml | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/aip_x1_1_launch/launch/pandar_node_container.launch.py b/aip_x1_1_launch/launch/pandar_node_container.launch.py index 4f51035b..fc102338 100644 --- a/aip_x1_1_launch/launch/pandar_node_container.launch.py +++ b/aip_x1_1_launch/launch/pandar_node_container.launch.py @@ -39,6 +39,7 @@ def get_pandar_monitor_info(): p = yaml.safe_load(f)["/**"]["ros__parameters"] return p + def get_vehicle_info(context): # TODO(TIER IV): Use Parameter Substitution after we drop Galactic support # https://github.com/ros2/launch_ros/blob/master/launch_ros/launch_ros/substitutions/parameter.py @@ -56,6 +57,7 @@ def get_vehicle_info(context): p["max_height_offset"] = gp["vehicle_height"] return p + def launch_setup(context, *args, **kwargs): def create_parameter_dict(*args): result = {} diff --git a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py index eab446b9..dd2a2c17 100644 --- a/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py +++ b/aip_x1_1_launch/launch/pointcloud_preprocessor.launch.py @@ -31,7 +31,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"), ], parameters=[ { diff --git a/common_sensor_launch/package.xml b/common_sensor_launch/package.xml index 7c636c0e..020add57 100644 --- a/common_sensor_launch/package.xml +++ b/common_sensor_launch/package.xml @@ -11,10 +11,10 @@ ament_cmake_auto dummy_diag_publisher - radar_tracks_msgs_converter radar_tracks_noise_filter velodyne_monitor + ament_lint_auto autoware_lint_common From b6a04b7eb70dd8ca064de26695a5310a7a06e9e8 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 5 Apr 2024 11:58:19 +0900 Subject: [PATCH 03/10] Add missing params to ground_segmentation.param.yaml Signed-off-by: Autumn60 --- .../ground_segmentation.param.yaml | 23 ++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index 0f3ea457..67fa0da5 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -35,13 +35,18 @@ # common_ground_filter: # plugin: "ground_segmentation::RayGroundFilterComponent" # parameters: - # initial_max_slope: 10.0 + # min_x: -0.01 + # max_x: +0.01 + # min_y: -0.01 + # max_y: +0.01 + # use_vehicle_footprint: True # general_max_slope: 10.0 # local_max_slope: 10.0 - # min_height_threshold: 0.3 + # initial_max_slope: 10.0 # radial_divider_angle: 1.0 + # min_height_threshold: 0.3 # concentric_divider_distance: 0.0 - # use_vehicle_footprint: True + # reclass_distance_threshold: 0.1 short_height_obstacle_detection_area_filter: parameters: @@ -55,13 +60,15 @@ ransac_ground_filter: parameters: - outlier_threshold: 0.1 - min_points: 400 - min_inliers: 200 +# base_frame: base_link + unit_axis: "z" max_iterations: 50 - height_threshold: 0.10 - plane_slope_threshold: 10.0 + min_trial: 200 + min_points: 400 + outlier_threshold: 0.1 + plane_slope_threshold: 10.0 voxel_size_x: 0.1 voxel_size_y: 0.1 voxel_size_z: 0.1 + height_threshold: 0.10 debug: False From eb68b118c38970e3191450fa3d073c4bb450f64b Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 5 Apr 2024 05:01:08 +0000 Subject: [PATCH 04/10] ci(pre-commit): autofix --- .../config/ground_segmentation/ground_segmentation.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index 67fa0da5..069f4302 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -66,7 +66,7 @@ min_trial: 200 min_points: 400 outlier_threshold: 0.1 - plane_slope_threshold: 10.0 + plane_slope_threshold: 10.0 voxel_size_x: 0.1 voxel_size_y: 0.1 voxel_size_z: 0.1 From 264653976f9cb79c763d3064c9063235f1893b4e Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 5 Apr 2024 14:49:35 +0900 Subject: [PATCH 05/10] Add missing params to ground_segmentation.param.yaml Signed-off-by: Autumn60 --- .../config/ground_segmentation/ground_segmentation.param.yaml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index 069f4302..9572591e 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -31,6 +31,10 @@ gnd_grid_buffer_size: 4 detection_range_z_max: 2.5 elevation_grid_mode: true + use_recheck_ground_cluster: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 # common_ground_filter: # plugin: "ground_segmentation::RayGroundFilterComponent" From 2d89f091109d0a260dfad8a71570bb3bd6cfe85c Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 5 Apr 2024 16:02:41 +0900 Subject: [PATCH 06/10] Fix crop_box params in ground_segmentation.param.yaml Signed-off-by: Autumn60 --- .../config/ground_segmentation/ground_segmentation.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index 9572591e..979648dd 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -13,8 +13,8 @@ max_x: 150.0 min_y: -70.0 max_y: 70.0 - margin_max_z: 0.0 # to extend the crop box max_z from vehicle_height - margin_min_z: -2.5 # to extend the crop box min_z from ground + margin_max_z: 2.5 # to extend the crop box max_z from vehicle_height + margin_min_z: -0.5 # to extend the crop box min_z from ground negative: False common_ground_filter: From 21e9329a0f2efc02d8920b1f6321c787c1e69554 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Fri, 5 Apr 2024 16:04:28 +0900 Subject: [PATCH 07/10] Fix crop_box param in ground_segmentation.param.yaml Signed-off-by: Autumn60 --- .../config/ground_segmentation/ground_segmentation.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index 979648dd..f46812f1 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -13,7 +13,7 @@ max_x: 150.0 min_y: -70.0 max_y: 70.0 - margin_max_z: 2.5 # to extend the crop box max_z from vehicle_height + margin_max_z: 0.0 # to extend the crop box max_z from vehicle_height margin_min_z: -0.5 # to extend the crop box min_z from ground negative: False From ee655c9d794b6bc7cb91ec38642a68c59dcb2cb6 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sat, 6 Apr 2024 10:25:24 +0900 Subject: [PATCH 08/10] Revert crop_box params Signed-off-by: Autumn60 --- .../ground_segmentation/ground_segmentation.param.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index f46812f1..a2bf210a 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -9,10 +9,10 @@ common_crop_box_filter: parameters: - min_x: -100.0 - max_x: 150.0 - min_y: -70.0 - max_y: 70.0 + min_x: -50.0 + max_x: 100.0 + min_y: -50.0 + max_y: 50.0 margin_max_z: 0.0 # to extend the crop box max_z from vehicle_height margin_min_z: -0.5 # to extend the crop box min_z from ground negative: False From b27ad43b5367e4656ba00adcdf81567f033b2205 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Sat, 6 Apr 2024 11:55:51 +0900 Subject: [PATCH 09/10] Add cropbox param calc to ground_segmentation.launch.py Signed-off-by: Autumn60 --- .../ground_segmentation.launch.py | 46 +++++++++++++++++-- 1 file changed, 42 insertions(+), 4 deletions(-) diff --git a/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py b/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py index 920a221a..8cc51174 100644 --- a/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py +++ b/aip_x1_1_launch/launch/ground_segmentation/ground_segmentation.launch.py @@ -70,6 +70,18 @@ def get_vehicle_mirror_info(self): return p def create_additional_pipeline(self, lidar_name): + max_z = ( + self.vehicle_info["max_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_max_z" + ] + ) + min_z = ( + self.vehicle_info["min_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_min_z" + ] + ) components = [] components.append( ComposableNode( @@ -84,8 +96,8 @@ def create_additional_pipeline(self, lidar_name): { "input_frame": LaunchConfiguration("base_frame"), "output_frame": LaunchConfiguration("base_frame"), - # "min_z": self.vehicle_info["min_height_offset"], - # "max_z": self.vehicle_info["max_height_offset"], + "max_z": max_z, + "min_z": min_z, }, self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"], ], @@ -116,6 +128,18 @@ def create_additional_pipeline(self, lidar_name): return components def create_ransac_pipeline(self): + max_z = ( + self.vehicle_info["max_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_max_z" + ] + ) + min_z = ( + self.vehicle_info["min_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_min_z" + ] + ) components = [] components.append( ComposableNode( @@ -131,6 +155,8 @@ def create_ransac_pipeline(self): { "input_frame": LaunchConfiguration("base_frame"), "output_frame": LaunchConfiguration("base_frame"), + "max_z": max_z, + "min_z": min_z, }, self.ground_segmentation_param["short_height_obstacle_detection_area_filter"][ "parameters" @@ -184,6 +210,18 @@ def create_ransac_pipeline(self): return components def create_common_pipeline(self, input_topic, output_topic): + max_z = ( + self.vehicle_info["max_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_max_z" + ] + ) + min_z = ( + self.vehicle_info["min_height_offset"] + + self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][ + "margin_min_z" + ] + ) components = [] components.append( ComposableNode( @@ -198,8 +236,8 @@ def create_common_pipeline(self, input_topic, output_topic): { "input_frame": LaunchConfiguration("base_frame"), "output_frame": LaunchConfiguration("base_frame"), - "min_z": self.vehicle_info["min_height_offset"], - "max_z": self.vehicle_info["max_height_offset"], + "max_z": max_z, + "min_z": min_z, }, self.ground_segmentation_param["common_crop_box_filter"]["parameters"], ], From 9d5ef966e4523414333bedd7e94e759b187c32f0 Mon Sep 17 00:00:00 2001 From: 1222-takeshi Date: Mon, 8 Apr 2024 13:50:53 +0900 Subject: [PATCH 10/10] chore(ground_segmentation): change margin_max_z for cropbox Signed-off-by: 1222-takeshi --- .../config/ground_segmentation/ground_segmentation.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml index a2bf210a..0cb5133c 100644 --- a/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml +++ b/aip_x1_1_launch/config/ground_segmentation/ground_segmentation.param.yaml @@ -13,7 +13,7 @@ max_x: 100.0 min_y: -50.0 max_y: 50.0 - margin_max_z: 0.0 # to extend the crop box max_z from vehicle_height + margin_max_z: 0.63 # to extend the crop box max_z from vehicle_height margin_min_z: -0.5 # to extend the crop box min_z from ground negative: False