Skip to content

Commit bd98deb

Browse files
committed
chore(ground_segmentation_launch): change max_z of cropbox filter to vehicle_height
Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
1 parent eea40ae commit bd98deb

File tree

1 file changed

+24
-0
lines changed

1 file changed

+24
-0
lines changed

launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py

+24
Original file line numberDiff line numberDiff line change
@@ -70,6 +70,18 @@ def get_vehicle_mirror_info(self):
7070
return p
7171

7272
def create_additional_pipeline(self, lidar_name):
73+
max_z = (
74+
self.vehicle_info["max_height_offset"]
75+
+ self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][
76+
"margin_max_z"
77+
]
78+
)
79+
min_z = (
80+
self.vehicle_info["min_height_offset"]
81+
- self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"][
82+
"margin_min_z"
83+
]
84+
)
7385
components = []
7486
components.append(
7587
ComposableNode(
@@ -84,6 +96,8 @@ def create_additional_pipeline(self, lidar_name):
8496
{
8597
"input_frame": LaunchConfiguration("base_frame"),
8698
"output_frame": LaunchConfiguration("base_frame"),
99+
"max_z": max_z,
100+
"min_z": min_z,
87101
},
88102
self.ground_segmentation_param[f"{lidar_name}_crop_box_filter"]["parameters"],
89103
],
@@ -206,6 +220,14 @@ def create_ransac_pipeline(self):
206220
return components
207221

208222
def create_common_pipeline(self, input_topic, output_topic):
223+
max_z = (
224+
self.vehicle_info["max_height_offset"]
225+
+ self.ground_segmentation_param["common_crop_box_filter"]["parameters"]["margin_max_z"]
226+
)
227+
min_z = (
228+
self.vehicle_info["min_height_offset"]
229+
- self.ground_segmentation_param["common_crop_box_filter"]["parameters"]["margin_min_z"]
230+
)
209231
components = []
210232
components.append(
211233
ComposableNode(
@@ -220,6 +242,8 @@ def create_common_pipeline(self, input_topic, output_topic):
220242
{
221243
"input_frame": LaunchConfiguration("base_frame"),
222244
"output_frame": LaunchConfiguration("base_frame"),
245+
"max_z": max_z,
246+
"min_z": min_z,
223247
},
224248
self.ground_segmentation_param["common_crop_box_filter"]["parameters"],
225249
],

0 commit comments

Comments
 (0)