Merge pull request #952 from YoshiRi/chore/add_radar_tracking_pipeline

chore: add far radar tracking and detection fusion pipeline
tier4 · Oct 17, 2023 · 2dea1c3 · 2dea1c3
2 parents 75f7e49 + 47cce2c
commit 2dea1c3
Show file tree

Hide file tree

Showing 6 changed files with 89 additions and 28 deletions.
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
@@ -50,6 +50,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)"/>
@@ -389,27 +393,30 @@
   <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>
 
   <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 unless="$(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
@@ -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
@@ -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
@@ -92,6 +92,13 @@
   <arg name="remove_unknown" default="true"/>
   <arg name="trust_distance" default="30.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"/>
@@ -176,6 +183,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>
 

diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml
@@ -65,7 +65,16 @@
       <!-- tracking module -->
       <group>
         <push-ros-namespace namespace="tracking"/>
-        <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/tracking/tracking.launch.xml"/>
+        <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/tracking/tracking.launch.xml">
+          <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>
       <!-- prediction module -->
       <group>

diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp
@@ -27,8 +27,12 @@
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
 #include <geometry_msgs/msg/polygon.hpp>
 #include <geometry_msgs/msg/vector3.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 #include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Transform.h>
+#include <tf2/convert.h>
+#include <tf2/transform_datatypes.h>
 #include <tf2/utils.h>
 
 #include <cmath>