From 285ec36fd7255d45f789e243cc799b5527f17683 Mon Sep 17 00:00:00 2001
From: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
Date: Fri, 12 Jan 2024 19:42:39 +0900
Subject: [PATCH] fix: fixed the camera-lidar calibrator that was broken due to
 some new parameters ant the new launcher configuration schmeme

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
---
 .../launch/calibrator.launch.xml                                | 2 ++
 .../launch/default_project/tag_based_pnp_calibrator.launch.xml  | 2 ++
 .../views/launcher_configuration_view.py                        | 2 +-
 3 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml b/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml
index dc46ada1..ed4f0430 100644
--- a/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml
+++ b/sensor/extrinsic_tag_based_pnp_calibrator/launch/calibrator.launch.xml
@@ -5,6 +5,7 @@
   <arg name="base_frame" default="base_link"/>
   <arg name="lidar_model" description="For supported lidar models check the lidartag repository"/>
   <arg name="use_receive_time" default="false" description="If sensor synchronization does not work, we can force the use of system time"/>
+  <arg name="use_rectified_image" default="true" description="We need to know if the image has been rectified so we can determine the use of K or P"/>
   <arg name="calibration_pairs" default="9" description="Number of lidar-image pairs for the calibration to converge"/>
   <arg name="calibration_pairs_min_distance" default="1.5" description="Minimum allowed between a new detection and current pairs"/>
 
@@ -32,6 +33,7 @@
     <param name="max_tag_distance" value="20.0"/>
     <param name="max_allowed_homography_error" value="0.5"/>
     <param name="use_receive_time" value="$(var use_receive_time)"/>
+    <param name="use_rectified_image" value="$(var use_rectified_image)"/>
 
     <param name="calibration_crossvalidation_training_ratio" value="0.7"/>
     <param name="calibration_convergence_min_pairs" value="$(var calibration_pairs)"/>
diff --git a/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml
index b9c7cf3d..93cae250 100644
--- a/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml
+++ b/sensor/new_extrinsic_calibration_manager/launch/default_project/tag_based_pnp_calibrator.launch.xml
@@ -6,6 +6,7 @@
   <arg name="camera_info_topic" default="/sensing/camera/camera0/camera_info"/>
   <arg name="pointcloud_topic" default="/sensing/lidar/top/pointcloud_raw"/>
   <arg name="lidar_model" default="velodyne_vls128" description="this lidar must be supported by the lidartag package"/>
+  <arg name="use_rectified_image" default="true" description="image_rect vs. raw"/>
 
   <let name="rviz_profile" value=""/>
   <let name="rviz_profile" value="$(find-pkg-share extrinsic_tag_based_pnp_calibrator)/rviz/default_profile.rviz"/>
@@ -23,6 +24,7 @@
     <arg name="pointcloud_topic" value="$(var pointcloud_topic)"/>
     <arg name="pointcloud_topic_ex" value="$(var camera_info_topic)"/>
     <arg name="lidar_model" value="$(var lidar_model)"/>
+    <arg name="use_rectified_image" value="$(var use_rectified_image)"/>
     <arg name="calibration_service_name" value="calibrate_camera_lidar"/>
   </include>
 
diff --git a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py
index 55aca765..6b2e1c90 100644
--- a/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py
+++ b/sensor/new_extrinsic_calibration_manager/new_extrinsic_calibration_manager/views/launcher_configuration_view.py
@@ -88,7 +88,7 @@ def __init__(self, project_name, calibrator_name):
 
             description = e.description if e.description != "no description given" else ""
 
-            if len(e.default_value) > 0:
+            if e.default_value is not None and len(e.default_value) > 0:
                 default_value = e.default_value[-1].text.replace(
                     " ", ""
                 )  # KL: not sure if should the first or last default value