From 4badde5381528cc1e8dd53adf2608327792d88cc Mon Sep 17 00:00:00 2001
From: Yoshi Ri <yoshiyoshidetteiu@gmail.com>
Date: Thu, 19 Dec 2024 08:34:08 +0900
Subject: [PATCH] feat(camera_lidar_fusion): enable to use camera 8,9,10 in
 image_projection_based_fusion nodes (#1697)

* fix: x2 gen2 use image_raw instaed image_rect_color

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* feat: enable 9 camera fusion in perception and detection launch

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* feat: fix camera_lidar_detector launch

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* feat: fix launch for 9 input

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* feat: fix missed launcher

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

---------

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
---
 .../detection/detection.launch.xml            | 12 ++++++-
 .../detector/camera_lidar_detector.launch.xml | 18 +++++++++--
 .../merger/camera_lidar_merger.launch.xml     |  3 ++
 .../camera_lidar_radar_merger.launch.xml      |  3 ++
 .../launch/perception.launch.xml              | 31 +++++++++++--------
 .../launch/roi_cluster_fusion.launch.xml      |  6 ++++
 .../roi_detected_object_fusion.launch.xml     |  6 ++++
 .../launch/roi_pointcloud_fusion.launch.xml   |  6 ++++
 8 files changed, 68 insertions(+), 17 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
index 754d07c6d99bf..86233ba5f1f0e 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
@@ -27,7 +27,9 @@
   <arg name="camera_info6" default=""/>
   <arg name="image_raw7" default=""/>
   <arg name="camera_info7" default=""/>
-  <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
+  <arg name="image_raw8" default=""/>
+  <arg name="camera_info8" default=""/>
+  <arg name="image_number" default="1" description="choose image raw number"/>
 
   <!-- Radar parameters -->
   <arg name="input/radar" default="/sensing/radar/detected_objects"/>
@@ -53,6 +55,8 @@
       <arg name="camera_info6" value="$(var camera_info6)"/>
       <arg name="image_raw7" value="$(var image_raw7)"/>
       <arg name="camera_info7" value="$(var camera_info7)"/>
+      <arg name="image_raw8" value="$(var image_raw8)"/>
+      <arg name="camera_info8" value="$(var camera_info8)"/>
       <arg name="image_number" value="$(var image_number)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
       <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
@@ -103,6 +107,8 @@
       <arg name="camera_info6" value="$(var camera_info6)"/>
       <arg name="image_raw7" value="$(var image_raw7)"/>
       <arg name="camera_info7" value="$(var camera_info7)"/>
+      <arg name="image_raw8" value="$(var image_raw8)"/>
+      <arg name="camera_info8" value="$(var camera_info8)"/>
       <arg name="image_number" value="$(var image_number)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
       <arg name="use_radar_tracking_fusion" value="$(var use_radar_tracking_fusion)"/>
@@ -134,6 +140,8 @@
       <arg name="camera_info6" value="$(var camera_info6)"/>
       <arg name="image_raw7" value="$(var image_raw7)"/>
       <arg name="camera_info7" value="$(var camera_info7)"/>
+      <arg name="image_raw8" value="$(var image_raw8)"/>
+      <arg name="camera_info8" value="$(var camera_info8)"/>
       <arg name="image_number" value="$(var image_number)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
       <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
@@ -171,6 +179,8 @@
       <arg name="camera_info6" value="$(var camera_info6)"/>
       <arg name="image_raw7" value="$(var image_raw7)"/>
       <arg name="camera_info7" value="$(var camera_info7)"/>
+      <arg name="image_raw8" value="$(var image_raw8)"/>
+      <arg name="camera_info8" value="$(var camera_info8)"/>
       <arg name="image_number" value="$(var image_number)"/>
       <arg name="use_object_filter" value="$(var use_object_filter)"/>
       <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
index fe0ba0e614e60..e9cb9d3d292a4 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
@@ -31,11 +31,14 @@
   <arg name="detection_rois5" default="/perception/object_recognition/detection/rois5"/>
   <arg name="image_raw6" default="/image_raw6"/>
   <arg name="camera_info6" default="/camera_info6"/>
-  <arg name="detection_rois6" default="/perception/object_recognition/detection/rois6"/>
+  <arg name="detection_rois6" default="/perception/object_recognition/detection/rois8"/>
   <arg name="image_raw7" default="/image_raw7"/>
   <arg name="camera_info7" default="/camera_info7"/>
-  <arg name="detection_rois7" default="/perception/object_recognition/detection/rois7"/>
-  <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
+  <arg name="detection_rois7" default="/perception/object_recognition/detection/rois9"/>
+  <arg name="image_raw8" default="/image_raw8"/>
+  <arg name="camera_info8" default="/camera_info8"/>
+  <arg name="detection_rois8" default="/perception/object_recognition/detection/rois10"/>
+  <arg name="image_number" default="1" description="choose image raw number"/>
 
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolox)/launch/multiple_yolox.launch.xml">
@@ -70,6 +73,8 @@
       <arg name="input/rois6" value="$(var detection_rois6)"/>
       <arg name="input/camera_info7" value="$(var camera_info7)"/>
       <arg name="input/rois7" value="$(var detection_rois7)"/>
+      <arg name="input/camera_info8" value="$(var camera_info8)"/>
+      <arg name="input/rois8" value="$(var detection_rois8)"/>
       <arg name="input/rois_number" value="$(var image_number)"/>
       <arg name="input/image0" value="$(var image_raw0)"/>
       <arg name="input/image1" value="$(var image_raw1)"/>
@@ -79,6 +84,7 @@
       <arg name="input/image5" value="$(var image_raw5)"/>
       <arg name="input/image6" value="$(var image_raw6)"/>
       <arg name="input/image7" value="$(var image_raw7)"/>
+      <arg name="input/image8" value="$(var image_raw8)"/>
       <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
       <arg name="output/objects" value="objects"/>
       <arg name="model_name" value="$(var lidar_detection_model)"/>
@@ -138,6 +144,8 @@
           <arg name="input/rois6" value="$(var detection_rois6)"/>
           <arg name="input/camera_info7" value="$(var camera_info7)"/>
           <arg name="input/rois7" value="$(var detection_rois7)"/>
+          <arg name="input/camera_info8" value="$(var camera_info8)"/>
+          <arg name="input/rois8" value="$(var detection_rois8)"/>
           <arg name="input/rois_number" value="$(var image_number)"/>
           <arg name="input/image0" value="$(var image_raw0)"/>
           <arg name="input/image1" value="$(var image_raw1)"/>
@@ -147,6 +155,7 @@
           <arg name="input/image5" value="$(var image_raw5)"/>
           <arg name="input/image6" value="$(var image_raw6)"/>
           <arg name="input/image7" value="$(var image_raw7)"/>
+          <arg name="input/image8" value="$(var image_raw8)"/>
           <arg name="input/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
           <arg name="output_clusters" value="clusters"/>
           <arg name="param_path" value="$(var roi_pointcloud_fusion_param_path)"/>
@@ -194,6 +203,8 @@
           <arg name="input/rois6" value="$(var detection_rois6)"/>
           <arg name="input/camera_info7" value="$(var camera_info7)"/>
           <arg name="input/rois7" value="$(var detection_rois7)"/>
+          <arg name="input/camera_info8" value="$(var camera_info8)"/>
+          <arg name="input/rois8" value="$(var detection_rois8)"/>
           <arg name="input/rois_number" value="$(var image_number)"/>
           <arg name="input/clusters" value="$(var input/clustering)"/>
           <arg name="input/image0" value="$(var image_raw0)"/>
@@ -204,6 +215,7 @@
           <arg name="input/image5" value="$(var image_raw5)"/>
           <arg name="input/image6" value="$(var image_raw6)"/>
           <arg name="input/image7" value="$(var image_raw7)"/>
+          <arg name="input/image8" value="$(var image_raw8)"/>
           <arg name="output/clusters" value="clusters"/>
           <arg name="param_path" value="$(var roi_cluster_fusion_param_path)"/>
         </include>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml
index 5bcfe49643dce..388da486ebb8a 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml
@@ -67,6 +67,8 @@
       <arg name="input/rois6" value="$(var detection_rois6)"/>
       <arg name="input/camera_info7" value="$(var camera_info7)"/>
       <arg name="input/rois7" value="$(var detection_rois7)"/>
+      <arg name="input/camera_info8" value="$(var camera_info8)"/>
+      <arg name="input/rois8" value="$(var detection_rois8)"/>
       <arg name="input/rois_number" value="$(var image_number)"/>
       <arg name="input/image0" value="$(var image_raw0)"/>
       <arg name="input/image1" value="$(var image_raw1)"/>
@@ -76,6 +78,7 @@
       <arg name="input/image5" value="$(var image_raw5)"/>
       <arg name="input/image6" value="$(var image_raw6)"/>
       <arg name="input/image7" value="$(var image_raw7)"/>
+      <arg name="input/image8" value="$(var image_raw8)"/>
       <arg name="input/objects" value="$(var camera_roi_fusion/input/objects)"/>
       <arg name="output/objects" value="$(var camera_roi_fusion/output/objects)"/>
       <arg name="param_path" value="$(var roi_detected_object_fusion_param_path)"/>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml
index 07ab78cd37f72..2fd75806cbc10 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml
@@ -90,6 +90,8 @@
       <arg name="input/rois6" value="$(var detection_rois6)"/>
       <arg name="input/camera_info7" value="$(var camera_info7)"/>
       <arg name="input/rois7" value="$(var detection_rois7)"/>
+      <arg name="input/camera_info8" value="$(var camera_info8)"/>
+      <arg name="input/rois8" value="$(var detection_rois8)"/>
       <arg name="input/rois_number" value="$(var image_number)"/>
       <arg name="input/image0" value="$(var image_raw0)"/>
       <arg name="input/image1" value="$(var image_raw1)"/>
@@ -99,6 +101,7 @@
       <arg name="input/image5" value="$(var image_raw5)"/>
       <arg name="input/image6" value="$(var image_raw6)"/>
       <arg name="input/image7" value="$(var image_raw7)"/>
+      <arg name="input/image8" value="$(var image_raw8)"/>
       <arg name="input/objects" value="$(var camera_roi_fusion/input/objects)"/>
       <arg name="output/objects" value="$(var camera_roi_fusion/output/objects)"/>
       <arg name="param_path" value="$(var roi_detected_object_fusion_param_path)"/>
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index dd80ab5b2cde7..60a05d64eeb7b 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -48,31 +48,34 @@
   <arg name="mode" default="camera_lidar_fusion" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
   <arg name="data_path" default="$(env HOME)/autoware_data" description="packages data and artifacts directory path"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `transfusion`, `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
-  <arg name="image_raw0" default="/sensing/camera/camera0/image_rect_color" description="image raw topic name"/>
+  <arg name="image_raw0" default="/sensing/camera/camera0/image_raw" description="image raw topic name"/>
   <arg name="camera_info0" default="/sensing/camera/camera0/camera_info" description="camera info topic name"/>
   <arg name="detection_rois0" default="/perception/object_recognition/detection/rois0" description="detection rois output topic name"/>
-  <arg name="image_raw1" default="/sensing/camera/camera1/image_rect_color"/>
+  <arg name="image_raw1" default="/sensing/camera/camera1/image_raw"/>
   <arg name="camera_info1" default="/sensing/camera/camera1/camera_info"/>
   <arg name="detection_rois1" default="/perception/object_recognition/detection/rois1"/>
-  <arg name="image_raw2" default="/sensing/camera/camera2/image_rect_color"/>
+  <arg name="image_raw2" default="/sensing/camera/camera2/image_raw"/>
   <arg name="camera_info2" default="/sensing/camera/camera2/camera_info"/>
   <arg name="detection_rois2" default="/perception/object_recognition/detection/rois2"/>
-  <arg name="image_raw3" default="/sensing/camera/camera3/image_rect_color"/>
+  <arg name="image_raw3" default="/sensing/camera/camera3/image_raw"/>
   <arg name="camera_info3" default="/sensing/camera/camera3/camera_info"/>
   <arg name="detection_rois3" default="/perception/object_recognition/detection/rois3"/>
-  <arg name="image_raw4" default="/sensing/camera/camera4/image_rect_color"/>
+  <arg name="image_raw4" default="/sensing/camera/camera4/image_raw"/>
   <arg name="camera_info4" default="/sensing/camera/camera4/camera_info"/>
   <arg name="detection_rois4" default="/perception/object_recognition/detection/rois4"/>
-  <arg name="image_raw5" default="/sensing/camera/camera5/image_rect_color"/>
+  <arg name="image_raw5" default="/sensing/camera/camera5/image_raw"/>
   <arg name="camera_info5" default="/sensing/camera/camera5/camera_info"/>
   <arg name="detection_rois5" default="/perception/object_recognition/detection/rois5"/>
-  <arg name="image_raw6" default="/sensing/camera/camera6/image_rect_color"/>
-  <arg name="camera_info6" default="/sensing/camera/camera6/camera_info"/>
-  <arg name="detection_rois6" default="/perception/object_recognition/detection/rois6"/>
-  <arg name="image_raw7" default="/sensing/camera/camera7/image_rect_color"/>
-  <arg name="camera_info7" default="/sensing/camera/camera7/camera_info"/>
-  <arg name="detection_rois7" default="/perception/object_recognition/detection/rois7"/>
-  <arg name="image_number" default="6" description="choose image raw number(1-8)"/>
+  <arg name="image_raw6" default="/sensing/camera/camera8/image_raw"/>
+  <arg name="camera_info6" default="/sensing/camera/camera8/camera_info"/>
+  <arg name="detection_rois6" default="/perception/object_recognition/detection/rois8"/>
+  <arg name="image_raw7" default="/sensing/camera/camera9/image_raw"/>
+  <arg name="camera_info7" default="/sensing/camera/camera9/camera_info"/>
+  <arg name="detection_rois7" default="/perception/object_recognition/detection/rois9"/>
+  <arg name="image_raw8" default="/sensing/camera/camera10/image_raw"/>
+  <arg name="camera_info8" default="/sensing/camera/camera10/camera_info"/>
+  <arg name="detection_rois8" default="/perception/object_recognition/detection/rois10"/>
+  <arg name="image_number" default="9" description="choose image raw number"/>
   <arg name="use_vector_map" default="true" description="use vector map in prediction"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_low_height_cropbox" default="true" description="use low height crop filter in clustering"/>
@@ -204,6 +207,8 @@
           <arg name="camera_info6" value="$(var camera_info6)"/>
           <arg name="image_raw7" value="$(var image_raw7)"/>
           <arg name="camera_info7" value="$(var camera_info7)"/>
+          <arg name="image_raw8" value="$(var image_raw8)"/>
+          <arg name="camera_info8" value="$(var camera_info8)"/>
           <arg name="image_number" value="$(var image_number)"/>
           <arg name="sync_param_path" value="$(var object_recognition_detection_fusion_sync_param_path)"/>
           <arg name="roi_cluster_fusion_param_path" value="$(var object_recognition_detection_roi_cluster_fusion_param_path)"/>
diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml
index f624b099fccb3..108b07c60bfd8 100644
--- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml
+++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml
@@ -16,6 +16,8 @@
   <arg name="input/camera_info6" default="/camera_info6"/>
   <arg name="input/rois7" default="rois7"/>
   <arg name="input/camera_info7" default="/camera_info7"/>
+  <arg name="input/rois8" default="rois8"/>
+  <arg name="input/camera_info8" default="/camera_info8"/>
   <arg name="input/clusters" default="clusters"/>
   <arg name="output/clusters" default="labeled_clusters"/>
   <arg name="param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_cluster_fusion.param.yaml"/>
@@ -30,6 +32,7 @@
   <arg name="input/image5" default="/image_raw5"/>
   <arg name="input/image6" default="/image_raw6"/>
   <arg name="input/image7" default="/image_raw7"/>
+  <arg name="input/image8" default="/image_raw8"/>
   <group>
     <node pkg="image_projection_based_fusion" exec="roi_cluster_fusion_node" name="roi_cluster_fusion" output="screen">
       <param name="rois_number" value="$(var input/rois_number)"/>
@@ -63,6 +66,9 @@
       <param name="input/rois7" value="$(var input/rois7)"/>
       <param name="input/camera_info7" value="$(var input/camera_info7)"/>
       <param name="input/image7" value="$(var input/image7)"/>
+      <param name="input/rois8" value="$(var input/rois8)"/>
+      <param name="input/camera_info8" value="$(var input/camera_info8)"/>
+      <param name="input/image8" value="$(var input/image8)"/>
     </node>
   </group>
 </launch>
diff --git a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml
index f11280b7f7c67..89c98714e944a 100644
--- a/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml
+++ b/perception/image_projection_based_fusion/launch/roi_detected_object_fusion.launch.xml
@@ -16,6 +16,8 @@
   <arg name="input/camera_info6" default="/camera_info6"/>
   <arg name="input/rois7" default="rois7"/>
   <arg name="input/camera_info7" default="/camera_info7"/>
+  <arg name="input/rois8" default="rois8"/>
+  <arg name="input/camera_info8" default="/camera_info8"/>
   <arg name="input/objects" default="objects"/>
   <arg name="output/objects" default="fused_objects"/>
   <arg name="param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_detected_object_fusion.param.yaml"/>
@@ -33,6 +35,7 @@
   <arg name="input/image5" default="/image_raw5"/>
   <arg name="input/image6" default="/image_raw6"/>
   <arg name="input/image7" default="/image_raw7"/>
+  <arg name="input/image8" default="/image_raw8"/>
 
   <group>
     <node pkg="image_projection_based_fusion" exec="roi_detected_object_fusion_node" name="roi_detected_object_fusion" output="screen">
@@ -67,6 +70,9 @@
       <param name="input/rois7" value="$(var input/rois7)"/>
       <param name="input/camera_info7" value="$(var input/camera_info7)"/>
       <param name="input/image7" value="$(var input/image7)"/>
+      <param name="input/rois8" value="$(var input/rois8)"/>
+      <param name="input/camera_info8" value="$(var input/camera_info8)"/>
+      <param name="input/image8" value="$(var input/image8)"/>
     </node>
   </group>
 </launch>
diff --git a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml
index cde06744aca58..30313246d82c6 100644
--- a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml
+++ b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml
@@ -16,6 +16,8 @@
   <arg name="input/camera_info6" default="/camera_info6"/>
   <arg name="input/rois7" default="rois7"/>
   <arg name="input/camera_info7" default="/camera_info7"/>
+  <arg name="input/rois8" default="rois8"/>
+  <arg name="input/camera_info8" default="/camera_info8"/>
   <arg name="input/pointcloud" default="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
   <arg name="output/pointcloud" default="roi_pointcloud_fusion/output/pointcloud"/>
   <arg name="output_clusters" default="output_clusters"/>
@@ -33,6 +35,7 @@
   <arg name="input/image5" default="/image_raw5"/>
   <arg name="input/image6" default="/image_raw6"/>
   <arg name="input/image7" default="/image_raw7"/>
+  <arg name="input/image8" default="/image_raw8"/>
   <group>
     <node pkg="image_projection_based_fusion" exec="roi_pointcloud_fusion_node" name="roi_pointcloud_fusion" output="screen">
       <param name="rois_number" value="$(var input/rois_number)"/>
@@ -68,6 +71,9 @@
       <param name="input/rois7" value="$(var input/rois7)"/>
       <param name="input/camera_info7" value="$(var input/camera_info7)"/>
       <param name="input/image7" value="$(var input/image7)"/>
+      <param name="input/rois8" value="$(var input/rois8)"/>
+      <param name="input/camera_info8" value="$(var input/camera_info8)"/>
+      <param name="input/image8" value="$(var input/image8)"/>
     </node>
   </group>
 </launch>