From cf405c37fd6561c19919e3a72ae9c3ee2e440c46 Mon Sep 17 00:00:00 2001
From: kotaro-hihara <kotaro.hihara@tier4.jp>
Date: Tue, 9 Jul 2024 15:08:37 +0900
Subject: [PATCH] read callibration from individual_params

Signed-off-by: kotaro-hihara <kotaro.hihara@tier4.jp>
---
 aip_x2_launch/launch/lidar.launch.xml          | 16 ++++++++--------
 .../launch/nebula_node_container.launch.py     | 18 +++---------------
 2 files changed, 11 insertions(+), 23 deletions(-)

diff --git a/aip_x2_launch/launch/lidar.launch.xml b/aip_x2_launch/launch/lidar.launch.xml
index c4297b05..4dc5dc87 100644
--- a/aip_x2_launch/launch/lidar.launch.xml
+++ b/aip_x2_launch/launch/lidar.launch.xml
@@ -37,7 +37,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/pandar/front_lower.csv" /-->
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
         <!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
@@ -70,7 +70,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/pandar/front_lower.csv" /-->
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/front_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
         <!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
@@ -103,7 +103,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/pandar/front_lower.csv" /-->
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
         <!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
@@ -136,7 +136,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/pandar/front_lower.csv" /-->
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/left_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
         <!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
@@ -169,7 +169,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/pandar/front_lower.csv" /-->
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
         <!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
@@ -202,7 +202,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/pandar/front_lower.csv" /-->
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/right_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
         <!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
@@ -235,7 +235,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/pandar/front_lower.csv" /-->
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_lower.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
         <!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
@@ -268,7 +268,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/pandar/front_lower.csv" /-->
+        <arg name="calibration_file" value="$(find-pkg-share individual_params)/config/$(var vehicle_id)/aip_x2/pandar/rear_upper.csv" />
         <arg name="launch_driver" value="$(var launch_driver)" />
         <!--arg name="dual_return_filter_param_file" value="$(var dual_return_filter_param_file)" /-->
         <arg name="vehicle_mirror_param_file" value="$(var vehicle_mirror_param_file)" />
diff --git a/aip_x2_launch/launch/nebula_node_container.launch.py b/aip_x2_launch/launch/nebula_node_container.launch.py
index 53e7140e..583fe211 100644
--- a/aip_x2_launch/launch/nebula_node_container.launch.py
+++ b/aip_x2_launch/launch/nebula_node_container.launch.py
@@ -27,7 +27,6 @@
 from launch_ros.descriptions import ComposableNode
 import yaml
 
-
 def get_lidar_make(sensor_name):
     if sensor_name[:6].lower() == "pandar":
         return "Hesai", ".csv"
@@ -75,18 +74,6 @@ def create_parameter_dict(*args):
     # Model and make
     sensor_model = LaunchConfiguration("sensor_model").perform(context)
     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)
 
     nodes = []
 
@@ -136,7 +123,6 @@ def create_parameter_dict(*args):
             name=sensor_make.lower() + "_driver_ros_wrapper_node",
             parameters=[
                 {
-                    "calibration_file": sensor_calib_fp,
                     "sensor_model": sensor_model,
                     **create_parameter_dict(
                         "host_ip",
@@ -157,6 +143,7 @@ def create_parameter_dict(*args):
                         "ptp_transport_type",
                         "ptp_switch_type",
                         "ptp_domain",
+                        "calibration_file"
                     ),
                     "launch_hw": True,
                 },
@@ -290,7 +277,6 @@ def create_parameter_dict(*args):
         parameters=[
             {
                 "sensor_model": sensor_model,
-                "calibration_file": sensor_calib_fp,
                 **create_parameter_dict(
                     "sensor_ip",
                     "host_ip",
@@ -309,6 +295,7 @@ def create_parameter_dict(*args):
                     "ptp_transport_type",
                     "ptp_switch_type",
                     "ptp_domain",
+                    "calibration_file",
                 ),
             }
         ],
@@ -360,6 +347,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg("use_multithread", "False", "use multithread")
     add_launch_arg("use_intra_process", "False", "use ROS 2 component container communication")
     add_launch_arg("container_name", "nebula_node_container")
+    add_launch_arg("calibration_file", "")
 
     set_container_executable = SetLaunchConfiguration(
         "container_executable",