Feature/radar fusion launch (tier4#1294)

* feat(tier4_perception_launch): add radar launcher (tier4#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 tier4#1263

* fix format

* fix default arg

* Revert "fix default arg"

This reverts commit 72b2690.

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

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>

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>

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>

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>

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>

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>

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 -->