From b27ad43b5367e4656ba00adcdf81567f033b2205 Mon Sep 17 00:00:00 2001
From: Autumn60 <akiro.harada@tier4.jp>
Date: Sat, 6 Apr 2024 11:55:51 +0900
Subject: [PATCH] Add cropbox param calc to ground_segmentation.launch.py

Signed-off-by: Autumn60 <akiro.harada@tier4.jp>
---
 .../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"],
                 ],