Merge pull request #949 from YoshiRi/chore/add_radar_pipeline

feat: add radar far detection pipeline
tier4 · Oct 17, 2023 · 1174025 · 1174025
2 parents 40c87d2 + 15d8f54
commit 1174025
Show file tree

Hide file tree

Showing 13 changed files with 189 additions and 126 deletions.
diff --git a/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <launch>
-  <!-- Interface param -->
+  <!-- Interface parameters -->
   <arg name="output/objects" default="objects"/>
 
-  <!-- LiDAR param -->
+  <!-- LiDAR parameters -->
   <arg name="input/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
   <arg name="container_name" default="pointcloud_container"/>
@@ -18,7 +18,7 @@
   <arg name="trust_distance" default="30.0"/>
   <arg name="use_roi_based_cluster" default="false"/>
 
-  <!-- Camera param -->
+  <!-- Camera parameters -->
   <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="detection_rois0" default="/perception/object_recognition/detection/rois0" description="detection rois output topic name"/>

diff --git a/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
@@ -1,23 +1,23 @@
 <?xml version="1.0"?>
 <launch>
-  <!-- Interface param -->
+  <!-- Interface parameters -->
   <arg name="output/objects" default="objects"/>
 
-  <!-- LiDAR param -->
+  <!-- LiDAR parameters -->
   <arg name="input/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
   <arg name="container_name" default="pointcloud_container"/>
 
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
-  <arg name="score_threshold" default="0.35"/>
+  <arg name="lidar_detection_score_threshold" default="0.35"/>
 
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
   <arg name="remove_unknown" default="true"/>
   <arg name="trust_distance" default="30.0"/>
 
-  <!-- Camera param -->
+  <!-- Camera parameters -->
   <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="detection_rois0" default="/perception/object_recognition/detection/rois0" description="detection rois output topic name"/>
@@ -44,8 +44,11 @@
   <arg name="detection_rois7" default="/perception/object_recognition/detection/rois7"/>
   <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
 
-  <!-- Radar param -->
+  <!-- Radar parameters -->
   <arg name="input/radar" default="/sensing/radar/detected_objects"/>
+  <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
+  <arg name="use_near_radar_fusion" default="false"/>
+  <arg name="far_object_merger_sync_queue_size" default="20"/>
 
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
@@ -60,7 +63,7 @@
     <arg name="image_number" value="$(var image_number)"/>
   </include> -->
 
-  <!-- Radar detection-->
+  <!-- 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">
@@ -102,6 +105,48 @@
       </include>
     </group>
 
+    <!-- roi based clustering -->
+    <group>
+      <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml" if="$(var use_roi_based_cluster)">
+        <arg name="input/camera_info0" value="$(var camera_info0)"/>
+        <arg name="input/rois0" value="$(var detection_rois0)"/>
+        <arg name="input/camera_info1" value="$(var camera_info1)"/>
+        <arg name="input/rois1" value="$(var detection_rois1)"/>
+        <arg name="input/camera_info2" value="$(var camera_info2)"/>
+        <arg name="input/rois2" value="$(var detection_rois2)"/>
+        <arg name="input/camera_info3" value="$(var camera_info3)"/>
+        <arg name="input/rois3" value="$(var detection_rois3)"/>
+        <arg name="input/camera_info4" value="$(var camera_info4)"/>
+        <arg name="input/rois4" value="$(var detection_rois4)"/>
+        <arg name="input/camera_info5" value="$(var camera_info5)"/>
+        <arg name="input/rois5" value="$(var detection_rois5)"/>
+        <arg name="input/camera_info6" value="$(var camera_info6)"/>
+        <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/rois_number" value="$(var image_number)"/>
+        <arg name="input/image0" value="$(var image_raw0)"/>
+        <arg name="input/image1" value="$(var image_raw1)"/>
+        <arg name="input/image2" value="$(var image_raw2)"/>
+        <arg name="input/image3" value="$(var image_raw3)"/>
+        <arg name="input/image4" value="$(var image_raw4)"/>
+        <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/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
+        <arg name="output_clusters" value="roi_cluster/clusters"/>
+      </include>
+    </group>
+
+    <!-- simple_cluster_merger -->
+    <group>
+      <include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml" if="$(var use_roi_based_cluster)">
+        <arg name="input/cluster0" value="euclidean_cluster/clusters"/>
+        <arg name="input/cluster1" value="roi_cluster/clusters"/>
+        <arg name="output/clusters" value="clusters"/>
+      </include>
+    </group>
+
     <group>
       <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
         <arg name="input/objects" value="clusters"/>
@@ -113,6 +158,8 @@
       <let name="input/clustering" value="/perception/object_recognition/detection/clustering/clusters"/>
       <push-ros-namespace namespace="camera_lidar_fusion"/>
       <!-- Fusion camera-lidar to classify -->
+
+      <!-- euclidean clustering -->
       <group>
         <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_cluster_fusion.launch.xml">
           <arg name="input/camera_info0" value="$(var camera_info0)"/>
@@ -141,54 +188,12 @@
           <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="output/clusters" value="euclidean_cluster/clusters"/>
+          <arg name="output/clusters" value="clusters"/>
           <arg name="remove_unknown" value="$(var remove_unknown)"/>
           <arg name="trust_distance" value="$(var trust_distance)"/>
         </include>
       </group>
 
-      <!-- roi based clustering -->
-      <group>
-        <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_pointcloud_fusion.launch.xml" if="$(var use_roi_based_cluster)">
-          <arg name="input/camera_info0" value="$(var camera_info0)"/>
-          <arg name="input/rois0" value="$(var detection_rois0)"/>
-          <arg name="input/camera_info1" value="$(var camera_info1)"/>
-          <arg name="input/rois1" value="$(var detection_rois1)"/>
-          <arg name="input/camera_info2" value="$(var camera_info2)"/>
-          <arg name="input/rois2" value="$(var detection_rois2)"/>
-          <arg name="input/camera_info3" value="$(var camera_info3)"/>
-          <arg name="input/rois3" value="$(var detection_rois3)"/>
-          <arg name="input/camera_info4" value="$(var camera_info4)"/>
-          <arg name="input/rois4" value="$(var detection_rois4)"/>
-          <arg name="input/camera_info5" value="$(var camera_info5)"/>
-          <arg name="input/rois5" value="$(var detection_rois5)"/>
-          <arg name="input/camera_info6" value="$(var camera_info6)"/>
-          <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/rois_number" value="$(var image_number)"/>
-          <arg name="input/image0" value="$(var image_raw0)"/>
-          <arg name="input/image1" value="$(var image_raw1)"/>
-          <arg name="input/image2" value="$(var image_raw2)"/>
-          <arg name="input/image3" value="$(var image_raw3)"/>
-          <arg name="input/image4" value="$(var image_raw4)"/>
-          <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/pointcloud" value="/perception/object_recognition/detection/pointcloud_map_filtered/pointcloud"/>
-          <arg name="output_clusters" value="roi_cluster/clusters"/>
-        </include>
-      </group>
-
-      <!-- simple_cluster_merger -->
-      <group>
-        <include file="$(find-pkg-share cluster_merger)/launch/cluster_merger.launch.xml" if="$(var use_roi_based_cluster)">
-          <arg name="input/cluster0" value="euclidean_cluster/clusters"/>
-          <arg name="input/cluster1" value="roi_cluster/clusters"/>
-          <arg name="output/clusters" value="clusters"/>
-        </include>
-      </group>
-
       <group>
         <include file="$(find-pkg-share shape_estimation)/launch/shape_estimation.launch.xml">
           <arg name="input/objects" value="clusters"/>
@@ -218,7 +223,7 @@
       <include file="$(find-pkg-share lidar_centerpoint)/launch/lidar_centerpoint.launch.xml">
         <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
         <arg name="output/objects" value="objects"/>
-        <arg name="score_threshold" value="$(var score_threshold)"/>
+        <arg name="score_threshold" value="$(var lidar_detection_score_threshold)"/>
         <arg name="model_name" value="$(var centerpoint_model_name)"/>
         <arg name="model_path" value="$(var centerpoint_model_path)"/>
         <arg name="model_param_path" value="$(var lidar_model_param_path)/$(var centerpoint_model_name).param.yaml"/>
@@ -290,17 +295,20 @@
     </include>
   </group>
 
-  <!-- Radar fusion-->
+  <!-- Radar fusion -->
   <group>
-    <include file="$(find-pkg-share radar_fusion_to_detected_object)/launch/radar_object_fusion_to_detected_object.launch.xml">
+    <include file="$(find-pkg-share radar_fusion_to_detected_object)/launch/radar_object_fusion_to_detected_object.launch.xml" if="$(var use_near_radar_fusion)">
       <arg name="input/objects" value="$(var lidar_detection_model)/objects"/>
       <arg name="input/radars" value="radar/noise_filtered_objects"/>
       <arg name="output/objects" value="radar_fusion/objects"/>
     </include>
   </group>
 
-  <!-- Camera late fusion-->
+  <!-- Camera late fusion -->
   <group>
+    <let name="target_objects" value="radar_fusion/objects" if="$(var use_near_radar_fusion)"/>
+    <let name="target_objects" value="$(var lidar_detection_model)/objects" unless="$(var use_near_radar_fusion)"/>
+
     <include file="$(find-pkg-share image_projection_based_fusion)/launch/roi_detected_object_fusion.launch.xml">
       <arg name="input/camera_info0" value="$(var camera_info0)"/>
       <arg name="input/rois0" value="$(var detection_rois0)"/>
@@ -327,7 +335,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/objects" value="radar_fusion/objects"/>
+      <arg name="input/objects" value="$(var target_objects)"/>
       <arg name="output/objects" value="$(var lidar_detection_model)/roi_fusion/objects"/>
     </include>
   </group>
@@ -381,16 +389,27 @@
   <group if="$(eval &quot;'$(var objects_filter_method)'=='lanelet_filter'&quot;)">
     <include file="$(find-pkg-share detected_object_validation)/launch/object_lanelet_filter.launch.xml" if="$(var use_object_filter)">
       <arg name="input/object" value="objects_before_filter"/>
-      <arg name="output/object" value="$(var output/objects)"/>
+      <arg name="output/object" value="near_objects"/>
       <arg name="filtering_range_param" value="$(var object_recognition_detection_object_lanelet_filter_param_path)"/>
     </include>
   </group>
 
   <group if="$(eval &quot;'$(var objects_filter_method)'=='position_filter'&quot;)">
     <include file="$(find-pkg-share detected_object_validation)/launch/object_position_filter.launch.xml" if="$(var use_object_filter)">
       <arg name="input/object" value="objects_before_filter"/>
-      <arg name="output/object" value="$(var output/objects)"/>
+      <arg name="output/object" value="near_objects"/>
       <arg name="filtering_range_param" value="$(var object_recognition_detection_object_position_filter_param_path)"/>
     </include>
   </group>
+
+  <!-- Merge far_objects and near_objects. This merger will be removed after tracking fusion operates stably. -->
+  <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
+    <arg name="input/object0" value="near_objects"/>
+    <arg name="input/object1" value="radar/far_objects"/>
+    <arg name="output/object" value="$(var output/objects)"/>
+    <arg name="priority_mode" value="0"/>
+    <arg name="data_association_matrix_path" value="$(var object_recognition_detection_object_merger_data_association_matrix_param_path)"/>
+    <arg name="distance_threshold_list_path" value="$(var object_recognition_detection_object_merger_distance_threshold_list_path)"/>
+    <arg name="sync_queue_size" value="$(var far_object_merger_sync_queue_size)"/>
+  </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
@@ -1,15 +1,16 @@
 <?xml version="1.0"?>
 <launch>
+  <!-- Interface parameters -->
   <arg name="mode" default="lidar" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
 
-  <!-- lidar param -->
+  <!-- LiDAR parameters -->
   <arg name="input/pointcloud"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
 
-  <!-- camera param -->
+  <!-- Camera parameters -->
   <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=""/>
@@ -28,14 +29,15 @@
   <arg name="camera_info7" default=""/>
   <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
 
-  <!-- camera lidar fusion param -->
+  <!-- Radar parameters -->
+  <arg name="input/radar" default="/sensing/radar/detected_objects"/>
+  <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
+
+  <!-- Camera-LiDAR fusion parameters -->
   <arg name="remove_unknown" default="true"/>
   <arg name="trust_distance" default="30.0"/>
 
-  <!-- radar param -->
-  <arg name="input/radar" default="/sensing/radar/detected_objects"/>
-
-  <!-- camera lidar radar fusion based detection-->
+  <!-- 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)"/>
@@ -63,13 +65,16 @@
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="remove_unknown" value="$(var remove_unknown)"/>
       <arg name="trust_distance" value="$(var trust_distance)"/>
-      <arg name="input/radar" value="$(var input/radar)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
+      <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
+      <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
+      <arg name="input/radar" value="$(var input/radar)"/>
+      <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
     </include>
   </group>
 
-  <!-- camera lidar fusion based detection-->
+  <!-- Camera-LiDAR fusion based detection -->
   <group if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_fusion&quot;')">
     <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)"/>
@@ -100,7 +105,7 @@
     </include>
   </group>
 
-  <!-- lidar radar based detection-->
+  <!-- LiDAR-Radar fusion 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)"/>
@@ -109,12 +114,14 @@
       <arg name="use_object_filter" value="$(var use_object_filter)"/>
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
-      <arg name="input/radar" value="$(var input/radar)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
+      <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
+      <arg name="input/radar" value="$(var input/radar)"/>
+      <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
     </include>
   </group>
 
-  <!-- lidar based detection-->
+  <!-- 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">
       <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
@@ -127,11 +134,12 @@
     </include>
   </group>
 
-  <!-- radar based detection-->
+  <!-- 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="input/radar" value="$(var input/radar)"/>
       <arg name="output/objects" value="objects"/>
+      <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
     </include>
   </group>
 </launch>