From c75d581016c559e7bd8a6bf3e63b173c13e30d63 Mon Sep 17 00:00:00 2001
From: Yoshi Ri <yoshiyoshidetteiu@gmail.com>
Date: Thu, 19 Oct 2023 17:18:14 +0900
Subject: [PATCH] feat(tier4_perception_launch): add parameter to control
 detection_by_tracker on/off (#5313)

* add parameter to control detection_by_tracker on/off

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

* style(pre-commit): autofix

* Update launch/tier4_perception_launch/launch/perception.launch.xml

Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>

---------

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>
---
 ...ra_lidar_fusion_based_detection.launch.xml | 19 ++++++++++++-----
 ...ar_radar_fusion_based_detection.launch.xml | 21 +++++++++++++------
 .../lidar_based_detection.launch.xml          | 19 ++++++++++++-----
 .../launch/perception.launch.xml              |  3 +++
 4 files changed, 46 insertions(+), 16 deletions(-)

diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index 67e123ea5b018..59fe3f13f1231 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -192,7 +192,7 @@
   </group>
 
   <!-- DetectionByTracker -->
-  <group>
+  <group if="$(var use_detection_by_tracker)">
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml"/>
   </group>
@@ -329,19 +329,24 @@
 
   <!-- Merger -->
   <group>
+    <!-- 1st merger to merge camera_lidar_fusion + ML lidar detection-->
     <let name="merger/input/objects" value="$(var lidar_detection_model)/validation/objects" if="$(var use_validator)"/>
     <let name="merger/input/objects" value="$(var lidar_detection_model)/objects" unless="$(var use_validator)"/>
+    <let name="detection_end_here" value="$(eval &quot;'$(var use_detection_by_tracker)'=='false' and '$(var use_object_filter)'=='false' &quot;)"/>
+    <let name="merger/output/objects" value="$(var output/objects)" if="$(var detection_end_here)"/>
+    <let name="merger/output/objects" value="$(var lidar_detection_model)_roi_cluster_fusion/objects" unless="$(var detection_end_here)"/>
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
       <arg name="input/object0" value="$(var merger/input/objects)"/>
       <arg name="input/object1" value="clustering/camera_lidar_fusion/objects"/>
-      <arg name="output/object" value="$(var lidar_detection_model)_roi_cluster_fusion/objects"/>
+      <arg name="output/object" value="$(var merger/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>
   </group>
 
-  <group>
+  <group if="$(var use_detection_by_tracker)">
+    <!-- 2nd merger to merge detection_by_tracker -->
     <let name="merger/output/objects" value="objects_before_filter" if="$(var use_object_filter)"/>
     <let name="merger/output/objects" value="$(var output/objects)" unless="$(var use_object_filter)"/>
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
@@ -356,16 +361,20 @@
 
   <!-- Filter -->
   <group if="$(eval &quot;'$(var objects_filter_method)'=='lanelet_filter'&quot;)">
+    <let name="filter/input/objects" value="objects_before_filter" if="$(var use_detection_by_tracker)"/>
+    <let name="filter/input/objects" value="$(var lidar_detection_model)_roi_cluster_fusion/objects" unless="$(var use_detection_by_tracker)"/>
     <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="input/object" value="$(var filter/input/objects)"/>
       <arg name="output/object" value="$(var output/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;)">
+    <let name="filter/input/objects" value="objects_before_filter" if="$(var use_detection_by_tracker)"/>
+    <let name="filter/input/objects" value="$(var lidar_detection_model)_roi_cluster_fusion/objects" unless="$(var use_detection_by_tracker)"/>
     <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="input/object" value="$(var filter/input/objects)"/>
       <arg name="output/object" value="$(var output/objects)"/>
       <arg name="filtering_range_param" value="$(var object_recognition_detection_object_position_filter_param_path)"/>
     </include>
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 07640495a5e19..11deaad5d06cc 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
@@ -217,7 +217,7 @@
   </group>
 
   <!-- DetectionByTracker -->
-  <group>
+  <group if="$(var use_detection_by_tracker)">
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml"/>
   </group>
@@ -367,21 +367,26 @@
 
   <!-- Merger -->
   <group>
+    <!-- 1st merger to merge camera_lidar_fusion + ML lidar detection-->
     <let name="merger/input/objects" value="$(var lidar_detection_model)/validation/objects" if="$(var use_validator)"/>
     <let name="merger/input/objects" value="$(var lidar_detection_model)/objects" unless="$(var use_validator)"/>
+    <let name="without_dbt_and_filter" value="$(eval &quot;'$(var use_detection_by_tracker)'=='false' and '$(var use_object_filter)'=='false' &quot;)"/>
+    <let name="merger/output/objects" value="near_objects" if="$(var without_dbt_and_filter)"/>
+    <let name="merger/output/objects" value="$(var lidar_detection_model)_roi_cluster_fusion/objects" unless="$(var without_dbt_and_filter)"/>
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
       <arg name="input/object0" value="$(var merger/input/objects)"/>
       <arg name="input/object1" value="clustering/camera_lidar_fusion/objects"/>
-      <arg name="output/object" value="$(var lidar_detection_model)_roi_cluster_fusion/objects"/>
+      <arg name="output/object" value="$(var merger/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>
   </group>
 
-  <group>
+  <group if="$(var use_detection_by_tracker)">
+    <!-- 2nd merger to merge detection_by_tracker -->
     <let name="merger/output/objects" value="objects_before_filter" if="$(var use_object_filter)"/>
-    <let name="merger/output/objects" value="$(var output/objects)" unless="$(var use_object_filter)"/>
+    <let name="merger/output/objects" value="near_objects" unless="$(var use_object_filter)"/>
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
       <arg name="input/object0" value="$(var lidar_detection_model)_roi_cluster_fusion/objects"/>
       <arg name="input/object1" value="detection_by_tracker/objects"/>
@@ -394,16 +399,20 @@
 
   <!-- Filter -->
   <group if="$(eval &quot;'$(var objects_filter_method)'=='lanelet_filter'&quot;)">
+    <let name="filter/input/objects" value="objects_before_filter" if="$(var use_detection_by_tracker)"/>
+    <let name="filter/input/objects" value="$(var lidar_detection_model)_roi_cluster_fusion/objects" unless="$(var use_detection_by_tracker)"/>
     <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="input/object" value="$(var filter/input/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>
 
   <group if="$(eval &quot;'$(var objects_filter_method)'=='position_filter'&quot;)">
+    <let name="filter/input/objects" value="objects_before_filter" if="$(var use_detection_by_tracker)"/>
+    <let name="filter/input/objects" value="$(var lidar_detection_model)_roi_cluster_fusion/objects" unless="$(var use_detection_by_tracker)"/>
     <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="input/object" value="$(var filter/input/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>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
index c844db39f4e9d..822c251ddad33 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -58,7 +58,7 @@
   </group>
 
   <!-- DetectionByTracker -->
-  <group>
+  <group if="$(var use_detection_by_tracker)">
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml"/>
   </group>
@@ -128,18 +128,23 @@
 
   <!-- Merger -->
   <group>
+    <!-- 1st merger to merge clustering + ML lidar detection-->
     <let name="merger/input/objects" value="$(var lidar_detection_model)/validation/objects" if="$(var use_validator)"/>
     <let name="merger/input/objects" value="$(var lidar_detection_model)/objects" unless="$(var use_validator)"/>
+    <let name="detection_end_here" value="$(eval &quot;'$(var use_detection_by_tracker)'=='false' and '$(var use_object_filter)'=='false' &quot;)"/>
+    <let name="merger/output/objects" value="temporary_merged_objects" unless="$(var detection_end_here)"/>
+    <let name="merger/output/objects" value="$(var output/objects)" if="$(var detection_end_here)"/>
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
       <arg name="input/object0" value="$(var merger/input/objects)"/>
       <arg name="input/object1" value="clustering/objects"/>
-      <arg name="output/object" value="temporary_merged_objects"/>
+      <arg name="output/object" value="$(var merger/output/objects)"/>
       <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>
   </group>
 
-  <group>
+  <group if="$(var use_detection_by_tracker)">
+    <!-- 2nd merger to merge detection_by_tracker -->
     <let name="merger/output/objects" value="objects_before_filter" if="$(var use_object_filter)"/>
     <let name="merger/output/objects" value="$(var output/objects)" unless="$(var use_object_filter)"/>
     <include file="$(find-pkg-share object_merger)/launch/object_association_merger.launch.xml">
@@ -154,16 +159,20 @@
 
   <!-- Filter -->
   <group if="$(eval &quot;'$(var objects_filter_method)'=='lanelet_filter'&quot;)">
+    <let name="filter/input/objects" value="objects_before_filter" if="$(var use_detection_by_tracker)"/>
+    <let name="filter/input/objects" value="temporary_merged_objects" unless="$(var use_detection_by_tracker)"/>
     <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="input/object" value="$(var filter/input/objects)"/>
       <arg name="output/object" value="$(var output/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;)">
+    <let name="filter/input/objects" value="objects_before_filter" if="$(var use_detection_by_tracker)"/>
+    <let name="filter/input/objects" value="temporary_merged_objects" unless="$(var use_detection_by_tracker)"/>
     <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="input/object" value="$(var filter/input/objects)"/>
       <arg name="output/object" value="$(var output/objects)"/>
       <arg name="filtering_range_param" value="$(var object_recognition_detection_object_position_filter_param_path)"/>
     </include>
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index 63ea0cc7f0f3d..347606d91759f 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -96,6 +96,9 @@
   <arg name="fusion_distance" default="100.0"/>
   <arg name="trust_object_distance" default="100.0"/>
 
+  <!-- Whether to use detection by tracker -->
+  <arg name="use_detection_by_tracker" default="true"/>
+
   <!-- Radar long range integration methods -->
   <arg
     name="use_radar_tracking_fusion"