From 02b08c50549d6829343fbef1fed861ee3566c8bc Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Wed, 6 Mar 2024 20:01:12 +0900 Subject: [PATCH] chore(ground_segmentation_launch): change max_z of cropbox filter to vehicle_height (#6549) * chore(ground_segmentation_launch): change max_z of cropbox filter to vehicle_height Signed-off-by: badai-nguyen * fix: typo Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- .../ground_segmentation.launch.py | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) 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 09d61823235d8..f502aef017979 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 @@ -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,6 +96,8 @@ def create_additional_pipeline(self, lidar_name): { "input_frame": LaunchConfiguration("base_frame"), "output_frame": LaunchConfiguration("base_frame"), + "max_z": max_z, + "min_z": min_z, }, self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"], ], @@ -206,6 +220,14 @@ 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["common_crop_box_filter"]["parameters"]["margin_max_z"] + ) + min_z = ( + self.vehicle_info["min_height_offset"] + + self.ground_segmentation_param["common_crop_box_filter"]["parameters"]["margin_min_z"] + ) components = [] components.append( ComposableNode( @@ -220,6 +242,8 @@ def create_common_pipeline(self, input_topic, output_topic): { "input_frame": LaunchConfiguration("base_frame"), "output_frame": LaunchConfiguration("base_frame"), + "max_z": max_z, + "min_z": min_z, }, self.ground_segmentation_param["common_crop_box_filter"]["parameters"], ],