Add cropbox param calc to ground_segmentation.launch.py

Signed-off-by: Autumn60 <[email protected]>
tier4 · Apr 6, 2024 · b27ad43 · b27ad43
1 parent ee655c9
commit b27ad43
Showing 1 changed file with 42 additions and 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
@@ -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"],
                 ],