From 81d61efc16b227ba43c68f23301b25744c35f3e1 Mon Sep 17 00:00:00 2001
From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com>
Date: Sat, 9 Jul 2022 15:02:44 +0900
Subject: [PATCH] Feature/radar fusion launch (#1294)

* feat(tier4_perception_launch): add radar launcher (#1263)

* feat(tier4_perception_launch): add radar launcher

Signed-off-by: scepter914 <scepter914@gmail.com>

* ci(pre-commit): autofix

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

* fix reviewed by #1263

* fix format

* fix default arg

* Revert "fix default arg"

This reverts commit 72b2690dc8cbd91fa5b14da091f4027c2c5fa661.

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
---
 ...ra_lidar_fusion_based_detection.launch.xml |  3 +-
 ...ar_radar_fusion_based_detection.launch.xml | 68 +++++++++++++++++++
 .../detection/detection.launch.xml            | 57 ++++++++++++++--
 .../lidar_based_detection.launch.xml          |  3 +-
 .../lidar_radar_based_detection.launch.xml    | 32 +++++++++
 .../radar_based_detection.launch.xml          | 12 ++++
 .../launch/perception.launch.xml              | 13 ++--
 7 files changed, 175 insertions(+), 13 deletions(-)
 create mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
 create mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
 create mode 100644 launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index 46ddadad521cd..81e7b2e097551 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -2,6 +2,7 @@
 <launch>
   <arg name="input/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
+  <arg name="output/objects" default="objects"/>
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
   <arg name="camera_info0" default="/camera_info" description="camera info topic name"/>
   <arg name="image_raw1" default="/image_raw1"/>
@@ -184,7 +185,7 @@
   <group>
     <include file="$(find-pkg-share detected_object_validation)/launch/object_lanelet_filter.launch.xml">
       <arg name="input/object" value="objects_before_filter"/>
-      <arg name="output/object" value="objects"/>
+      <arg name="output/object" value="$(var output/objects)"/>
     </include>
   </group>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
new file mode 100644
index 0000000000000..95cdaae066674
--- /dev/null
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- lidar param -->
+  <arg name="input/pointcloud"/>
+  <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
+  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+
+  <!-- camera param -->
+  <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
+  <arg name="camera_info0" default="/camera_info" description="camera info topic name"/>
+  <arg name="image_raw1" default="/image_raw1"/>
+  <arg name="camera_info1" default="/camera_info1"/>
+  <arg name="image_raw2" default="/image_raw2"/>
+  <arg name="camera_info2" default="/camera_info2"/>
+  <arg name="image_raw3" default="/image_raw3"/>
+  <arg name="camera_info3" default="/camera_info3"/>
+  <arg name="image_raw4" default="/image_raw4"/>
+  <arg name="camera_info4" default="/camera_info4"/>
+  <arg name="image_raw5" default="/image_raw5"/>
+  <arg name="camera_info5" default="/camera_info5"/>
+  <arg name="image_raw6" default="/image_raw6"/>
+  <arg name="camera_info6" default="/camera_info6"/>
+  <arg name="image_raw7" default="/image_raw7"/>
+  <arg name="camera_info7" default="/camera_info7"/>
+  <arg name="image_number" default="1" description="choose image raw number(0-7)"/>
+
+  <!-- camera lidar fusion based detection-->
+  <group>
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml">
+      <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+      <arg name="output/objects" value="camera_lidar_fusion/objects"/>
+      <arg name="image_raw0" value="$(var image_raw0)"/>
+      <arg name="camera_info0" value="$(var camera_info0)"/>
+      <arg name="image_raw1" value="$(var image_raw1)"/>
+      <arg name="camera_info1" value="$(var camera_info1)"/>
+      <arg name="image_raw2" value="$(var image_raw2)"/>
+      <arg name="camera_info2" value="$(var camera_info2)"/>
+      <arg name="image_raw3" value="$(var image_raw3)"/>
+      <arg name="camera_info3" value="$(var camera_info3)"/>
+      <arg name="image_raw4" value="$(var image_raw4)"/>
+      <arg name="camera_info4" value="$(var camera_info4)"/>
+      <arg name="image_raw5" value="$(var image_raw5)"/>
+      <arg name="camera_info5" value="$(var camera_info5)"/>
+      <arg name="image_raw6" value="$(var image_raw6)"/>
+      <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_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)"/>
+    </include>
+  </group>
+
+  <!-- radar detection-->
+  <group>
+    <push-ros-namespace namespace="radar"/>
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/radar_based_detection.launch.xml">
+      <arg name="output/objects" value="front_center/objects"/>
+    </include>
+  </group>
+
+  <!-- camera lidar radar fusion-->
+  <include file="$(find-pkg-share radar_fusion_to_detected_object)/launch/radar_object_fusion_to_detected_object.launch.xml">
+    <arg name="input/objects" value="camera_lidar_fusion/objects"/>
+    <arg name="input/radars" value="radar/objects"/>
+    <arg name="output/objects" value="objects"/>
+  </include>
+</launch>
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 6a975a2874627..19afdf22d9cbd 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
@@ -1,8 +1,13 @@
 <?xml version="1.0"?>
 <launch>
+  <arg name="mode" default="lidar" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
+
+  <!-- lidar param -->
   <arg name="input/pointcloud"/>
-  <arg name="mode" description="options: `camera_lidar_fusion`, `lidar` or `camera`"/>
-  <arg name="lidar_detection_model" default="apollo" description="options: `centerpoint`, `apollo`"/>
+  <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
+  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+
+  <!-- camera param -->
   <arg name="image_raw0" default="/image_raw" description="image raw topic name"/>
   <arg name="camera_info0" default="/camera_info" description="camera info topic name"/>
   <arg name="image_raw1" default=""/>
@@ -20,7 +25,34 @@
   <arg name="image_raw7" default=""/>
   <arg name="camera_info7" default=""/>
   <arg name="image_number" default="1" description="choose image raw number(0-7)"/>
-  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+
+  <!-- radar param -->
+
+  <!-- camera lidar radar fusion based detection-->
+  <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_radar_fusion&quot;')">
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml">
+      <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+      <arg name="image_raw0" value="$(var image_raw0)"/>
+      <arg name="camera_info0" value="$(var camera_info0)"/>
+      <arg name="image_raw1" value="$(var image_raw1)"/>
+      <arg name="camera_info1" value="$(var camera_info1)"/>
+      <arg name="image_raw2" value="$(var image_raw2)"/>
+      <arg name="camera_info2" value="$(var camera_info2)"/>
+      <arg name="image_raw3" value="$(var image_raw3)"/>
+      <arg name="camera_info3" value="$(var camera_info3)"/>
+      <arg name="image_raw4" value="$(var image_raw4)"/>
+      <arg name="camera_info4" value="$(var camera_info4)"/>
+      <arg name="image_raw5" value="$(var image_raw5)"/>
+      <arg name="camera_info5" value="$(var camera_info5)"/>
+      <arg name="image_raw6" value="$(var image_raw6)"/>
+      <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_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)"/>
+    </include>
+  </group>
 
   <!-- camera lidar fusion based detection-->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_fusion&quot;')">
@@ -47,6 +79,16 @@
       <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
     </include>
   </group>
+
+  <!-- lidar radar based detection-->
+  <group if="$(eval '&quot;$(var mode)&quot;==&quot;lidar_radar_fusion&quot;')">
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml">
+      <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+      <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+    </include>
+  </group>
+
   <!-- lidar based detection-->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;lidar&quot;')">
     <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/lidar_based_detection.launch.xml">
@@ -55,6 +97,11 @@
       <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
     </include>
   </group>
-  <!-- camera based detection-->
-  <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera&quot;')"/>
+
+  <!-- radar based detection-->
+  <group if="$(eval '&quot;$(var mode)&quot;==&quot;radar&quot;')">
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/radar_based_detection.launch.xml">
+      <arg name="output/objects" value="objects"/>
+    </include>
+  </group>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
index e65f8162e2d49..d36b9d6265c34 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -2,6 +2,7 @@
 <launch>
   <arg name="input/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
+  <arg name="output/objects" default="objects"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
@@ -107,7 +108,7 @@
   <group>
     <include file="$(find-pkg-share detected_object_validation)/launch/object_lanelet_filter.launch.xml">
       <arg name="input/object" value="objects_before_filter"/>
-      <arg name="output/object" value="objects"/>
+      <arg name="output/object" value="$(var output/objects)"/>
     </include>
   </group>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
new file mode 100644
index 0000000000000..8b7e9a3089fa0
--- /dev/null
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- lidar param -->
+  <arg name="input/pointcloud"/>
+  <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
+  <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
+
+  <!-- lidar based detection-->
+  <group>
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/lidar_based_detection.launch.xml">
+      <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+      <arg name="output/objects" value="lidar/objects"/>
+      <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
+      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
+    </include>
+  </group>
+
+  <!-- radar detection-->
+  <group>
+    <push-ros-namespace namespace="radar"/>
+    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/radar_based_detection.launch.xml">
+      <arg name="output/objects" value="objects"/>
+    </include>
+  </group>
+
+  <!-- lidar radar fusion-->
+  <include file="$(find-pkg-share radar_fusion_to_detected_object)/launch/radar_object_fusion_to_detected_object.launch.xml">
+    <arg name="input/objects" value="lidar/objects"/>
+    <arg name="input/radars" value="radar/objects"/>
+    <arg name="output/objects" value="objects"/>
+  </include>
+</launch>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml
new file mode 100644
index 0000000000000..21de187f126e5
--- /dev/null
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="output/objects" default="objects"/>
+
+  <include file="$(find-pkg-share radar_tracks_msgs_converter)/launch/radar_tracks_msgs_converter.launch.xml">
+    <arg name="input/radar_objects" value="/sensing/radar/front_center/objects_raw"/>
+    <arg name="input/odometry" value="/localization/kinematic_state"/>
+    <arg name="output/radar_objects" value="$(var output/objects)"/>
+    <arg name="update_rate_hz" value="20.0"/>
+    <arg name="use_twist_compensation" value="true"/>
+  </include>
+</launch>
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index 51ee075f27d0e..50dd53550f20c 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -1,9 +1,10 @@
 <?xml version="1.0"?>
 <launch>
-  <arg name="vehicle_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml" description="path to the file of vehicle info yaml"/>
   <!-- common parameters -->
   <arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud" description="The topic will be used in the detection module"/>
-  <arg name="mode" description="options: `camera_lidar_fusion`, `lidar` or `camera`"/>
+  <arg name="vehicle_param_file" default="$(find-pkg-share vehicle_info_util)/config/vehicle_info.param.yaml" description="path to the file of vehicle info yaml"/>
+  <arg name="mode" default="camera_lidar_fusion" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
+  <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`"/>
   <arg name="image_raw0" default="/sensing/camera/camera0/image_rect_color" description="image raw topic name"/>
   <arg name="camera_info0" default="/sensing/camera/camera0/camera_info" description="camera info topic name"/>
   <arg name="image_raw1" default="/sensing/camera/camera1/image_rect_color"/>
@@ -45,6 +46,7 @@
         <arg name="use_multithread" value="true"/>
         <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
         <arg name="container_name" value="$(var pointcloud_container_name)"/>
+        <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
       </include>
     </group>
 
@@ -53,7 +55,7 @@
       <push-ros-namespace namespace="occupancy_grid_map"/>
       <include file="$(find-pkg-share tier4_perception_launch)/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py">
         <arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/single_frame/pointcloud_raw"/>
-        <arg name="input/raw_pointcloud" value="/sensing/lidar/concatenated/pointcloud"/>
+        <arg name="input/raw_pointcloud" value="$(var input/pointcloud)"/>
         <arg name="output" value="/perception/occupancy_grid_map/map"/>
         <arg name="use_intra_process" value="true"/>
         <arg name="use_multithread" value="true"/>
@@ -69,8 +71,9 @@
       <group>
         <push-ros-namespace namespace="detection"/>
         <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detection.launch.xml">
-          <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
           <arg name="mode" value="$(var mode)"/>
+          <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
+          <arg name="lidar_detection_model" value="$(var lidar_detection_model)"/>
           <arg name="image_raw0" value="$(var image_raw0)"/>
           <arg name="camera_info0" value="$(var camera_info0)"/>
           <arg name="image_raw1" value="$(var image_raw1)"/>
@@ -89,8 +92,6 @@
           <arg name="camera_info7" value="$(var camera_info7)"/>
           <arg name="image_number" value="$(var image_number)"/>
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
-          <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
-          <arg name="container_name" value="$(var pointcloud_container_name)"/>
         </include>
       </group>
       <!-- tracking module -->