fix(tier4_perception_launch): add object_merger of far_objects to fus…

…ion for Camera-LiDAR-Radar fusion (autowarefoundation#5026) * fix(tier4_perception_launch): add object_merger of far_objects to fusion for Camera-LiDAR-Radar fusion Signed-off-by: scepter914 <[email protected]> * fix conflict Signed-off-by: scepter914 <[email protected]> --------- Signed-off-by: scepter914 <[email protected]>
zulfaqar-azmi-t4 · Sep 19, 2023 · cca5fee · cca5fee
1 parent 51fa0f5
commit cca5fee
Show file tree

Hide file tree

Showing 7 changed files with 79 additions and 55 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,9 @@
   <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"/>
 
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
@@ -60,7 +61,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">
@@ -218,7 +219,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,7 +291,7 @@
     </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">
       <arg name="input/objects" value="$(var lidar_detection_model)/objects"/>
@@ -299,7 +300,7 @@
     </include>
   </group>
 
-  <!-- Camera late fusion-->
+  <!-- Camera late fusion -->
   <group>
     <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)"/>
@@ -381,16 +382,26 @@
   <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)"/>
+  </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>
diff --git a/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -8,7 +8,7 @@
   <arg name="use_pointcloud_container" default="false" description="use pointcloud container for detection preprocessor"/>
   <arg name="container_name" default="pointcloud_container"/>
   <arg name="use_validator" default="true" description="use obstacle_pointcloud based validator"/>
-  <arg name="score_threshold" default="0.35"/>
+  <arg name="lidar_detection_score_threshold" default="0.35"/>
   <arg name="centerpoint_model_name" default="centerpoint_tiny"/>
   <arg name="centerpoint_model_path" default="$(find-pkg-share lidar_centerpoint)/data"/>
   <arg name="lidar_model_param_path" default="$(find-pkg-share lidar_centerpoint)/config"/>
@@ -70,7 +70,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"/>

diff --git a/...ception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/...ception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
@@ -1,17 +1,18 @@
 <?xml version="1.0"?>
 <launch>
-  <!-- lidar param -->
+  <!-- LiDAR parameters -->
   <arg name="input/pointcloud"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `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"/>
-  <arg name="score_threshold" default="0.20"/>
+  <arg name="lidar_detection_score_threshold" default="0.35"/>
 
-  <!-- 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"/>
 
-  <!-- lidar based detection-->
+  <!-- LiDAR 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)"/>
@@ -22,11 +23,11 @@
       <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="score_threshold" value="$(var score_threshold)"/>
+      <arg name="lidar_detection_score_threshold" value="$(var lidar_detection_score_threshold)"/>
     </include>
   </group>
 
-  <!-- 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">
@@ -42,7 +43,7 @@
     </include>
   </group>
 
-  <!-- lidar radar fusion-->
+  <!-- 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/noise_filtered_objects"/>

diff --git a/...r4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/...r4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml
@@ -2,14 +2,14 @@
 <launch>
   <arg name="input/radar" default="/sensing/radar/detected_objects"/>
   <arg name="output/objects" default="far_objects"/>
-  <arg name="filter/angle_threshold" default="1.0472"/>
-  <arg name="filter/velocity_threshold" default="3.0"/>
-  <arg name="split/velocity_threshold" default="4.5"/>
-  <arg name="split_range" default="70.0"/>
-  <arg name="clustering/angle_threshold" default="0.174"/>
+  <arg name="filter/angle_threshold" default="1.221"/>
+  <arg name="filter/velocity_threshold" default="1.5"/>
+  <arg name="split/velocity_threshold" default="5.5"/>
+  <arg name="split_range" default="80.0"/>
+  <arg name="clustering/angle_threshold" default="0.349"/>
   <arg name="clustering/distance_threshold" default="10.0"/>
-  <arg name="clustering/velocity_threshold" default="4.0"/>
-  <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_position_filter.param.yaml"/>
+  <arg name="clustering/velocity_threshold" default="5.0"/>
+  <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
 
   <!-- Detection for far dynamic objects -->
   <include file="$(find-pkg-share radar_crossing_objects_noise_filter)/launch/radar_crossing_objects_noise_filter.launch.xml">