From 09cae006ed0a858c162329f361f13d1071b54b23 Mon Sep 17 00:00:00 2001
From: Tomohito Ando <tomohito.ando@tier4.jp>
Date: Wed, 2 Oct 2024 15:38:17 +0900
Subject: [PATCH] feat: read LiDAR calibration params from file

---
 aip_x2_gen2_launch/launch/hesai_OT128.launch.xml |  4 ++--
 aip_x2_gen2_launch/launch/hesai_QT128.launch.xml |  4 ++--
 aip_x2_gen2_launch/launch/lidar.launch.xml       | 16 ++++++++--------
 .../launch/nebula_node_container.launch.py       | 15 +++------------
 4 files changed, 15 insertions(+), 24 deletions(-)

diff --git a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml
index e8881a1f..1c5a2707 100644
--- a/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml
+++ b/aip_x2_gen2_launch/launch/hesai_OT128.launch.xml
@@ -26,7 +26,7 @@
   <arg name="is_channel_order_top2down" default="true"/>
   <arg name="horizontal_resolution" default="0.4"/>
   <arg name="enable_blockage_diag" default="$(var enable_blockage_diag)"/>
-  <arg name="calibration" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_lower.csv" />
+  <arg name="calibration_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_lower.csv" />
   <arg name="dual_return_filter_param_file" default="$(var dual_return_filter_param_file)" />
   <arg name="vehicle_mirror_param_file" default="$(var vehicle_mirror_param_file)" />
   <arg name="blockage_diagnostics_param_file" default="$(find-pkg-share aip_x2_gen2_launch)/config/blockage_diagnostics_param_file.yaml" />
@@ -61,7 +61,7 @@
     <arg name="is_channel_order_top2down" value="$(var is_channel_order_top2down)"/>
     <arg name="horizontal_resolution" value="$(var horizontal_resolution)"/>
     <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
-    <arg name="calibration" value="$(var calibration)"/>
+    <arg name="calibration_file" value="$(var calibration_file)"/>
     <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)"/>
     <arg name="blockage_diagnostics_param_file" value="$(var blockage_diagnostics_param_file)"/>
 
diff --git a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml
index 59df827e..4a29e9c0 100644
--- a/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml
+++ b/aip_x2_gen2_launch/launch/hesai_QT128.launch.xml
@@ -27,7 +27,7 @@
   <arg name="is_channel_order_top2down" default="true"/>
   <arg name="horizontal_resolution" default="0.4"/>
   <arg name="enable_blockage_diag" default="$(var enable_blockage_diag)"/>
-  <arg name="calibration" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_lower.csv" />
+  <arg name="calibration_file" default="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_lower.csv" />
   <arg name="dual_return_filter_param_file" default="$(var dual_return_filter_param_file)" />
   <arg name="vehicle_mirror_param_file" default="$(var vehicle_mirror_param_file)" />
   <arg name="blockage_diagnostics_param_file" default="$(find-pkg-share aip_x2_gen2_launch)/config/blockage_diagnostics_param_file.yaml" />
@@ -61,7 +61,7 @@
     <arg name="is_channel_order_top2down" value="$(var is_channel_order_top2down)"/>
     <arg name="horizontal_resolution" value="$(var horizontal_resolution)"/>
     <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
-    <arg name="calibration" value="$(var calibration)"/>
+    <arg name="calibration_file" value="$(var calibration_file)"/>
     <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)"/>
     <arg name="blockage_diagnostics_param_file" value="$(var blockage_diagnostics_param_file)"/>
   </include>
diff --git a/aip_x2_gen2_launch/launch/lidar.launch.xml b/aip_x2_gen2_launch/launch/lidar.launch.xml
index 203845ce..6655c3d8 100644
--- a/aip_x2_gen2_launch/launch/lidar.launch.xml
+++ b/aip_x2_gen2_launch/launch/lidar.launch.xml
@@ -53,7 +53,7 @@
         <arg name="is_channel_order_top2down" value="true"/>
         <arg name="horizontal_resolution" value="0.4"/>
         <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
-        <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_lower.csv" />
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/rear_upper.csv" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
       </include>
     </group>
@@ -86,7 +86,7 @@
         <arg name="is_channel_order_top2down" value="true"/>
         <arg name="horizontal_resolution" value="0.4"/>
         <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
-        <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_lower.csv" />
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/left_upper.csv" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
       </include>
     </group>
@@ -119,7 +119,7 @@
         <arg name="is_channel_order_top2down" value="true"/>
         <arg name="horizontal_resolution" value="0.4"/>
         <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
-        <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_lower.csv" />
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/rear_lower.csv" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
       </include>
     </group>
@@ -152,7 +152,7 @@
         <arg name="is_channel_order_top2down" value="true"/>
         <arg name="horizontal_resolution" value="0.4"/>
         <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
-        <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_lower.csv" />
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/left_lower.csv" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
       </include>
     </group>
@@ -186,7 +186,7 @@
         <arg name="is_channel_order_top2down" value="true"/>
         <arg name="horizontal_resolution" value="0.4"/>
         <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
-        <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_lower.csv" />
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_upper.csv" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
       </include>
     </group>
@@ -219,7 +219,7 @@
         <arg name="is_channel_order_top2down" value="true"/>
         <arg name="horizontal_resolution" value="0.4"/>
         <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
-        <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_lower.csv" />
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/right_upper.csv" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
       </include>
     </group>
@@ -252,7 +252,7 @@
         <arg name="is_channel_order_top2down" value="true"/>
         <arg name="horizontal_resolution" value="0.4"/>
         <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
-        <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_lower.csv" />
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_lower.csv" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
       </include>
     </group>
@@ -285,7 +285,7 @@
         <arg name="is_channel_order_top2down" value="true"/>
         <arg name="horizontal_resolution" value="0.4"/>
         <arg name="enable_blockage_diag" value="$(var enable_blockage_diag)"/>
-        <arg name="calibration" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/front_lower.csv" />
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2_gen2/pandar/right_lower.csv" />
         <arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" />
       </include>
     </group>
diff --git a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py
index e09ff11a..c1746dc4 100644
--- a/aip_x2_gen2_launch/launch/nebula_node_container.launch.py
+++ b/aip_x2_gen2_launch/launch/nebula_node_container.launch.py
@@ -85,17 +85,6 @@ def str2vector(string):
     sensor_make, sensor_extension = get_lidar_make(sensor_model)
     nebula_decoders_share_dir = get_package_share_directory("nebula_decoders")
 
-    # Calibration file
-    sensor_calib_fp = os.path.join(
-        nebula_decoders_share_dir,
-        "calibration",
-        sensor_make.lower(),
-        sensor_model + sensor_extension,
-    )
-    assert os.path.exists(
-        sensor_calib_fp
-    ), "Sensor calib file under calibration/ was not found: {}".format(sensor_calib_fp)
-
     glog_component = ComposableNode(
         package="glog_component",
         plugin="GlogComponent",
@@ -108,7 +97,6 @@ def str2vector(string):
         name=sensor_make.lower() + "_ros_wrapper_node",
         parameters=[
             {
-                "calibration_file": sensor_calib_fp,
                 "sensor_model": sensor_model,
                 "point_filters": "{}",
                 **create_parameter_dict(
@@ -134,6 +122,7 @@ def str2vector(string):
                     "ptp_switch_type",
                     "ptp_domain",
                     "diag_span",
+                    "calibration_file",
                 ),
                 "launch_hw": True,
                 "retry_hw": True,
@@ -383,6 +372,8 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("max_azimuth_deg", "225.0")
     add_launch_arg("enable_blockage_diag", "true")
 
+    add_launch_arg("calibration_file", "")
+
     set_container_executable = SetLaunchConfiguration(
         "container_executable",
         "component_container",