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..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 @@ -9,12 +9,12 @@ common_crop_box_filter: parameters: - 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 + min_x: -50.0 + max_x: 100.0 + min_y: -50.0 + max_y: 50.0 + 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 common_ground_filter: @@ -31,17 +31,26 @@ 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" # 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 +64,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 + 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 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"], ],