From fb35f6216ae57ec72ff30f173198d2d70ba99f31 Mon Sep 17 00:00:00 2001
From: Yoshi Ri <yoshiyoshidetteiu@gmail.com>
Date: Tue, 17 Oct 2023 00:13:17 +0900
Subject: [PATCH] feat(tier4_perception_launch): add radar far object
 integration in tracking stage (#5269)

* update tracking/perception launch

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* switch tracker launcher mode with argument

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* update prediction to switch by radar_long_range_integration paramter

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* make radar far object integration switchable between detection/tracking

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* fix camera lidar radar fusion flow when 'tracking' is used.

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* fix spelling and appearance

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* reconstruct topic flow when use tracking to merge far object detection and near object detection

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* fix input topic miss in tracking.launch

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* fix comment in camera_lidar_radar fusion

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* refactor: rename and remove paramters in prediction.launch

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* refactor: rename merger control variable from string to bool

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

---------

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
---
 ...ar_radar_fusion_based_detection.launch.xml | 31 ++++++-----
 .../prediction/prediction.launch.xml          |  3 +-
 .../tracking/tracking.launch.xml              | 53 ++++++++++++++-----
 .../launch/perception.launch.xml              | 15 ++++++
 4 files changed, 75 insertions(+), 27 deletions(-)

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
index fbe5f21c041b6..7d0988635fb10 100644
--- 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
@@ -51,6 +51,10 @@
   <arg name="use_near_radar_fusion" default="false"/>
   <arg name="far_object_merger_sync_queue_size" default="20"/>
 
+  <!-- Filter output name. Switch output topic name by 'use_radar_tracking_fusion' parameter defined in perception.launch -->
+  <let name="output_of_filtered_objects" value="$(var output/objects)" if="$(var use_radar_tracking_fusion)"/>
+  <let name="output_of_filtered_objects" value="near_objects" unless="$(var use_radar_tracking_fusion)"/>
+
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolo)/launch/yolo.launch.xml">
     <arg name="image_raw0" value="$(var image_raw0)"/>
@@ -392,7 +396,7 @@
   <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="near_objects"/>
+      <arg name="output/object" value="$(var output_of_filtered_objects)"/>
       <arg name="filtering_range_param" value="$(var object_recognition_detection_object_lanelet_filter_param_path)"/>
     </include>
   </group>
@@ -400,19 +404,22 @@
   <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="near_objects"/>
+      <arg name="output/object" value="$(var output_of_filtered_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>
+  <!-- Merge far_objects and near_objects in detection stage.
+      Control parameter 'use_radar_tracking_fusion' should defined in perception.launch.xml -->
+  <group if="$(var use_radar_tracking_fusion)">
+    <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>
+  </group>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml
index 711e6374f5786..fa343f49840ad 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml
@@ -1,12 +1,13 @@
 <?xml version="1.0"?>
 <launch>
   <arg name="use_vector_map" default="false" description="use vector map in prediction"/>
-  <arg name="param_path" default="$(find-pkg-share map_based_prediction)/config/map_based_prediction.param.yaml"/>
+  <arg name="input/objects" default="/perception/object_recognition/tracking/objects"/>
 
   <group if="$(var use_vector_map)">
     <set_remap from="objects" to="/perception/object_recognition/objects"/>
     <include file="$(find-pkg-share map_based_prediction)/launch/map_based_prediction.launch.xml">
       <arg name="output_topic" value="/perception/object_recognition/objects"/>
+      <arg name="input_topic" value="$(var input/objects)"/>
       <arg name="param_path" value="$(var object_recognition_prediction_map_based_prediction_param_path)"/>
     </include>
   </group>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml
index 77de1e5995ee0..07b53fb671732 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml
@@ -3,29 +3,54 @@
   <arg name="mode" default="lidar" description="options: `camera_lidar_radar_fusion`, `camera_lidar_fusion`, `lidar_radar_fusion`, `lidar` or `radar`"/>
   <arg name="publish_rate" default="10.0"/>
   <arg name="enable_delay_compensation" default="true"/>
+
+  <!-- Radar Tracking and Merger parameters -->
+  <arg name="use_radar_tracking_fusion" default="false" description="use radar tracking fusion"/>
   <arg name="radar_tracker_input" default="/perception/object_recognition/detection/radar/far_objects"/>
   <arg name="radar_tracker_output" default="/perception/object_recognition/tracking/radar/far_objects"/>
+  <arg name="object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path" description="association param file for radar far object tracking"/>
+  <arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path" description="tracking setting param file for radar far object tracking"/>
+  <arg name="object_recognition_tracking_radar_object_tracker_node_param_path" description="node param file for radar far object tracking"/>
+  <arg name="object_recognition_tracking_object_merger_data_association_matrix_param_path" description="association param file for radar and lidar object merger"/>
+  <arg name="object_recognition_tracking_object_merger_node_param_path" description="node param file for radar and lidar object merger"/>
 
-  <group>
+  <!-- Run without tracking merger-->
+  <group unless="$(var use_radar_tracking_fusion)">
     <!--multi object tracking-->
     <include file="$(find-pkg-share multi_object_tracker)/launch/multi_object_tracker.launch.xml">
       <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path)"/>
       <arg name="publish_rate" value="$(var publish_rate)"/>
       <arg name="enable_delay_compensation" value="$(var enable_delay_compensation)"/>
     </include>
+  </group>
 
-    <!--radar long range dynamic object tracking if mode contains radar input-->
-    <!--
-    <group if="$(eval '&quot;$(var mode)&quot;==&quot;radar&quot; or &quot;$(var mode)&quot;==&quot;lidar_radar_fusion&quot; or &quot;$(var mode)&quot;==&quot;camera_lidar_radar_fusion&quot;')">
-      <include file="$(find-pkg-share radar_object_tracker)/launch/radar_object_tracker.launch.xml">
-        <arg name="publish_rate" value="$(var publish_rate)"/>
-        <arg name="enable_delay_compensation" value="$(var enable_delay_compensation)"/>
-        <arg name="input" value="$(var radar_tracker_input)"/>
-        <arg name="output" value="$(var radar_tracker_output)"/>
-        <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path)"/>
-        <arg name="tracker_setting_path" value="$(var object_recognition_tracking_radar_object_tracker_tracking_setting_param_path)"/>
-      </include>
-    </group>
-    -->
+  <!-- Run with tracking merger to add far radar information -->
+  <group if="$(var use_radar_tracking_fusion)">
+    <!--multi object tracking for near objects-->
+    <include file="$(find-pkg-share multi_object_tracker)/launch/multi_object_tracker.launch.xml">
+      <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path)"/>
+      <arg name="publish_rate" value="$(var publish_rate)"/>
+      <arg name="enable_delay_compensation" value="$(var enable_delay_compensation)"/>
+      <arg name="output" value="/perception/object_recognition/tracking/near_objects"/>
+    </include>
+
+    <!--radar long range dynamic object tracking-->
+    <include file="$(find-pkg-share radar_object_tracker)/launch/radar_object_tracker.launch.xml">
+      <arg name="publish_rate" value="$(var publish_rate)"/>
+      <arg name="enable_delay_compensation" value="$(var enable_delay_compensation)"/>
+      <arg name="input" value="$(var radar_tracker_input)"/>
+      <arg name="output" value="$(var radar_tracker_output)"/>
+      <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path)"/>
+      <arg name="tracker_setting_path" value="$(var object_recognition_tracking_radar_object_tracker_tracking_setting_param_path)"/>
+    </include>
+
+    <!--tracking object merger to merge near objects and far objects -->
+    <include file="$(find-pkg-share tracking_object_merger)/launch/decorative_tracker_merger.launch.xml">
+      <arg name="input/main_object" value="/perception/object_recognition/tracking/near_objects"/>
+      <arg name="input/sub_object" value="$(var radar_tracker_output)"/>
+      <arg name="output" value="/perception/object_recognition/tracking/objects"/>
+      <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_object_merger_data_association_matrix_param_path)"/>
+      <arg name="node_param_file_path" value="$(var object_recognition_tracking_object_merger_node_param_path)"/>
+    </include>
   </group>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index bbb7ebb2d8d25..63ea0cc7f0f3d 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -96,6 +96,13 @@
   <arg name="fusion_distance" default="100.0"/>
   <arg name="trust_object_distance" default="100.0"/>
 
+  <!-- Radar long range integration methods -->
+  <arg
+    name="use_radar_tracking_fusion"
+    default="false"
+    description="if use_radar_tracking_fusion:=true, radar information is merged in tracking launch. Otherwise, radar information is merged in detection launch."
+  />
+
   <!-- Perception module -->
   <group>
     <push-ros-namespace namespace="perception"/>
@@ -181,6 +188,14 @@
         <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)"/>
+          <arg
+            name="object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path"
+            value="$(var object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path)"
+          />
+          <arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path" value="$(var object_recognition_tracking_radar_object_tracker_tracking_setting_param_path)"/>
+          <arg name="object_recognition_tracking_radar_object_tracker_node_param_path" value="$(var object_recognition_tracking_radar_object_tracker_node_param_path)"/>
+          <arg name="object_recognition_tracking_object_merger_data_association_matrix_param_path" value="$(var object_recognition_tracking_object_merger_data_association_matrix_param_path)"/>
+          <arg name="object_recognition_tracking_object_merger_node_param_path" value="$(var object_recognition_tracking_object_merger_node_param_path)"/>
         </include>
       </group>