fix(tier4_perception_launch): add object_merger of far_objects to fusion for Camera-LiDAR-Radar fusion

launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml

@@ -9,7 +9,7 @@
   <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_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_downsample_pointcloud" default="true" description="use downsample pointcloud in perception"/>
@@ -48,6 +48,7 @@
 
   <!-- Radar param -->
   <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_position_filter.param.yaml"/>
 
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
@@ -220,7 +221,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"/>
@@ -383,16 +384,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>

launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml

@@ -1,16 +1,17 @@
 <?xml version="1.0"?>
 <launch>
+  <!-- Interface param -->
   <arg name="mode" default="lidar" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
 
-  <!-- lidar param -->
+  <!-- LiDAR param -->
   <arg name="input/pointcloud"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <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 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=""/>
@@ -29,14 +30,15 @@
   <arg name="camera_info7" default=""/>
   <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
 
-  <!-- camera lidar fusion param -->
+  <!-- Radar param -->
+  <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_position_filter.param.yaml"/>
+
+  <!-- Camera-LiDAR fusion param -->
   <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)"/>
@@ -65,10 +67,11 @@
       <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="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)"/>
@@ -97,7 +100,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)"/>
@@ -107,6 +110,7 @@
       <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="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
     </include>
   </group>
 
@@ -122,11 +126,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>

launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml

@@ -10,7 +10,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"/>
@@ -74,7 +74,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"/>

launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml

@@ -1,18 +1,19 @@
 <?xml version="1.0"?>
 <launch>
-  <!-- lidar param -->
+  <!-- LiDAR param -->
   <arg name="input/pointcloud"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `clustering`"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <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 param -->
   <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_position_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"/>

launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml

@@ -2,13 +2,13 @@
 <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="clustering/velocity_threshold" default="5.0"/>
   <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_position_filter.param.yaml"/>
 
   <!-- Detection for far dynamic objects -->

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -13,6 +13,7 @@
   <arg name="object_recognition_detection_object_merger_distance_threshold_list_path"/>
   <arg name="object_recognition_detection_fusion_sync_param_path"/>
   <arg name="object_recognition_detection_lidar_model_param_path"/>
+  <arg name="object_recognition_detection_radar_lanelet_filtering_range_param"/>
   <arg name="object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path"/>
   <arg name="object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path"/>
   <arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path"/>
@@ -24,11 +25,11 @@
   <arg name="occupancy_grid_map_updater_param_path"/>
   <arg name="traffic_light_arbiter_param_path"/>
 
-  <!-- centerpoint model configs -->
+  <!-- CenterPoint model configs -->
   <arg name="centerpoint_model_name" default="centerpoint_tiny" description="options: `centerpoint` or `centerpoint_tiny`"/>
   <arg name="centerpoint_model_path" default="$(find-pkg-share lidar_centerpoint)/data"/>
 
-  <!-- common parameters -->
+  <!-- Common parameters -->
   <arg name="input/pointcloud" default="/sensing/lidar/concatenated/pointcloud" description="The topic will be used in the detection module"/>
   <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`, `pointpainting`, `clustering`"/>
@@ -70,6 +71,7 @@
   <arg name="pointcloud_container_name" default="pointcloud_container"/>
   <arg name="objects_filter_method" default="lanelet_filter"/>
   <arg name="objects_validation_method" default="obstacle_pointcloud"/>
+
   <!-- Traffic Light Recognition Parameters -->
   <arg name="use_traffic_light_recognition" default="true"/>
   <arg name="traffic_light_recognition/enable_fine_detection" default="true"/>
@@ -88,15 +90,15 @@
     description="options: `traffic_light_classifier_mobilenetv2_batch_*` or `traffic_light_classifier_efficientNet_b1_batch_*`. The batch number must be either one of 1, 4, 6"
   />
 
-  <!-- Camera Lidar Fusion Parameters -->
+  <!-- Camera-Lidar Fusion Parameters -->
   <arg name="remove_unknown" default="true"/>
   <arg name="trust_distance" default="30.0"/>
 
-  <!-- perception module -->
+  <!-- Perception module -->
   <group>
     <push-ros-namespace namespace="perception"/>
 
-    <!-- object segmentation module -->
+    <!-- Object segmentation module -->
     <group>
       <push-ros-namespace namespace="obstacle_segmentation"/>
       <include file="$(find-pkg-share tier4_perception_launch)/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py">
@@ -109,7 +111,7 @@
       </include>
     </group>
 
-    <!-- occupancy grid map module -->
+    <!-- Occupancy grid map module -->
     <group>
       <push-ros-namespace namespace="occupancy_grid_map"/>
       <include file="$(find-pkg-share tier4_perception_launch)/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml">
@@ -127,10 +129,10 @@
       </include>
     </group>
 
-    <!-- object recognition module -->
+    <!-- Object recognition module -->
     <group unless="$(var use_empty_dynamic_object_publisher)">
       <push-ros-namespace namespace="object_recognition"/>
-      <!-- detection module -->
+      <!-- Detection module -->
       <group>
         <push-ros-namespace namespace="detection"/>
         <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/detection.launch.xml">
@@ -161,6 +163,7 @@
           <arg name="outlier_param_path" value="$(var object_recognition_detection_outlier_param_path)"/>
           <arg name="voxel_grid_param_path" value="$(var object_recognition_detection_voxel_grid_based_euclidean_param_path)"/>
           <arg name="voxel_grid_based_euclidean_param_path" value="$(var object_recognition_detection_voxel_grid_based_euclidean_cluster_param_path)"/>
+          <arg name="radar_lanelet_filtering_range_param" value="$(var object_recognition_detection_radar_lanelet_filtering_range_param)"/>
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
           <arg name="use_downsample_pointcloud" value="$(var use_downsample_pointcloud)"/>
           <arg name="use_object_filter" value="$(var use_object_filter)"/>
@@ -171,14 +174,15 @@
         </include>
       </group>
 
-      <!-- tracking module -->
+      <!-- Tracking module -->
       <group>
         <push-ros-namespace namespace="tracking"/>
         <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/tracking/tracking.launch.xml">
           <arg name="mode" value="$(var mode)"/>
         </include>
       </group>
-      <!-- prediction module -->
+
+      <!-- Prediction module -->
       <group>
         <push-ros-namespace namespace="prediction"/>
         <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/prediction/prediction.launch.xml">
@@ -194,7 +198,7 @@
       </node>
     </group>
 
-    <!-- traffic light module -->
+    <!-- Traffic light module -->
     <group if="$(var use_traffic_light_recognition)">
       <push-ros-namespace namespace="traffic_light_recognition"/>
       <include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light.launch.xml">