feat(tier4_perception_launch): enable multi channel tracker merger (a…

…utowarefoundation#7459) * feat: introduce multi channel tracker merger Signed-off-by: Taekjin LEE <[email protected]> feat: separate filters feat: filtering camera lidar fusion fix: object validator to modular Signed-off-by: Taekjin LEE <[email protected]> fix: add missing config Signed-off-by: Taekjin LEE <[email protected]> fix: radar only mode for both fusion mode fix Signed-off-by: Taekjin LEE <[email protected]> style(pre-commit): autofix Signed-off-by: Taekjin LEE <[email protected]> * fix: implement merger switching Signed-off-by: Taekjin LEE <[email protected]> * chore: move pointcloud filter from detection to filter group Signed-off-by: Taekjin LEE <[email protected]> * chore: define external and internal interfaces Signed-off-by: Taekjin LEE <[email protected]> * fix: set output of camera-lidar in absolute path Signed-off-by: Taekjin LEE <[email protected]> * chore: explicit object detection output Signed-off-by: Taekjin LEE <[email protected]> * style(pre-commit): autofix Signed-off-by: Taekjin LEE <[email protected]> * chore: update object detection input paths fix radar output Signed-off-by: Taekjin LEE <[email protected]> * chore: update object detection input paths Signed-off-by: Taekjin LEE <[email protected]> * fix: radar pipeline output Signed-off-by: Taekjin LEE <[email protected]> * chore: update object detection input paths This commit updates the input paths for object detection. It ensures that the correct paths are used for the detection process. Signed-off-by: Taekjin LEE <[email protected]> * style(pre-commit): autofix Signed-off-by: Taekjin LEE <[email protected]> * fix: group to avoid argument mixture Signed-off-by: Taekjin LEE <[email protected]> --------- Signed-off-by: Taekjin LEE <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tby-udel · Jul 14, 2024 · 20bdba6 · 20bdba6
1 parent 8152952
commit 20bdba6
Show file tree

Hide file tree

Showing 9 changed files with 241 additions and 133 deletions.
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
diff --git a/...tion_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/...tion_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml
@@ -1,17 +1,11 @@
 <?xml version="1.0"?>
 <launch>
-  <!-- Lidar parameters -->
-  <arg name="input/pointcloud"/>
-  <arg name="input/pointcloud_map/pointcloud"/>
-  <arg name="input/obstacle_segmentation/pointcloud"/>
-  <arg name="node/pointcloud_container"/>
-
   <!-- Lidar + Camera detector parameters -->
   <arg name="lidar_detection_model" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="use_roi_based_cluster"/>
   <arg name="use_low_intensity_cluster_filter"/>
 
-  <!-- Camera parameters -->
+  <!-- External interfaces -->
   <arg name="number_of_cameras"/>
   <arg name="input/camera0/image"/>
   <arg name="input/camera0/info"/>
@@ -38,6 +32,14 @@
   <arg name="input/camera7/info"/>
   <arg name="input/camera7/rois"/>
 
+  <arg name="node/pointcloud_container"/>
+  <arg name="input/pointcloud"/>
+  <arg name="input/pointcloud_map/pointcloud"/>
+  <arg name="input/obstacle_segmentation/pointcloud"/>
+
+  <arg name="output/ml_detector/objects"/>
+  <arg name="output/rule_detector/objects"/>
+
   <!-- Jetson AGX -->
   <!-- <include file="$(find-pkg-share tensorrt_yolox)/launch/multiple_yolox.launch.xml">
     <arg name="image_raw0" value="$(var input/camera0/image)"/>
@@ -81,7 +83,7 @@
       <arg name="input/image6" value="$(var input/camera6/image)"/>
       <arg name="input/image7" value="$(var input/camera7/image)"/>
       <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
-      <arg name="output/objects" value="objects"/>
+      <arg name="output/objects" value="$(var output/ml_detector/objects)"/>
 
       <arg name="model_name" value="$(var lidar_detection_model)"/>
       <arg name="model_path" value="$(var pointpainting_model_path)"/>
@@ -93,18 +95,6 @@
     </include>
   </group>
 
-  <!-- Pointcloud filter -->
-  <group>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py">
-      <arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
-      <arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
-      <arg name="use_intra_process" value="true"/>
-      <arg name="use_multithread" value="true"/>
-      <arg name="pointcloud_container_name" value="$(var node/pointcloud_container)"/>
-      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
-    </include>
-  </group>
-
   <!-- Clustering -->
   <group>
     <push-ros-namespace namespace="clustering"/>
@@ -230,7 +220,7 @@
       <group>
         <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
           <arg name="input" value="objects_with_feature"/>
-          <arg name="output" value="objects"/>
+          <arg name="output" value="$(var output/rule_detector/objects)"/>
         </include>
       </group>
     </group>

diff --git a/...ception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/...ception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml
@@ -1,18 +1,19 @@
 <?xml version="1.0"?>
 <launch>
   <!-- Lidar parameters -->
-  <arg name="input/pointcloud"/>
-  <arg name="node/pointcloud_container"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `transfusion`, `centerpoint`, `apollo`, `clustering`"/>
-
   <!-- Lidar detector centerpoint parameters -->
   <arg name="centerpoint_model_name" default="centerpoint_tiny" description="options: `centerpoint`, `centerpoint_tiny` or `centerpoint_sigma`"/>
   <arg name="centerpoint_model_path" default="$(var data_path)/lidar_centerpoint"/>
-
   <!-- Lidar detector transfusion parameters -->
   <arg name="transfusion_model_name" default="transfusion" description="options: `transfusion`"/>
   <arg name="transfusion_model_path" default="$(var data_path)/lidar_transfusion"/>
 
+  <!-- External interfaces -->
+  <arg name="node/pointcloud_container"/>
+  <arg name="input/pointcloud"/>
+  <arg name="output/objects"/>
+
   <!-- TransFusion -->
   <group if="$(eval &quot;'$(var lidar_detection_model)'=='transfusion'&quot;)">
     <push-ros-namespace namespace="transfusion"/>
@@ -21,7 +22,7 @@
     <group>
       <include file="$(find-pkg-share lidar_transfusion)/launch/lidar_transfusion.launch.xml">
         <arg name="input/pointcloud" value="$(var input/pointcloud)"/>
-        <arg name="output/objects" value="objects"/>
+        <arg name="output/objects" value="$(var output/objects)"/>
         <arg name="model_name" value="$(var transfusion_model_name)"/>
         <arg name="model_path" value="$(var transfusion_model_path)"/>
         <arg name="model_param_path" value="$(var lidar_model_param_path)/$(var transfusion_model_name).param.yaml"/>
@@ -41,7 +42,7 @@
     <group>
       <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="output/objects" value="$(var output/objects)"/>
         <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"/>
@@ -77,7 +78,7 @@
     <group>
       <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
         <arg name="input" value="objects_with_feature"/>
-        <arg name="output" value="objects"/>
+        <arg name="output" value="$(var output/objects)"/>
       </include>
     </group>
   </group>

diff --git a/...eption_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/...eption_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml
@@ -1,22 +1,10 @@
 <?xml version="1.0"?>
 <launch>
-  <!-- Lidar parameters -->
+  <!-- External interfaces -->
+  <arg name="node/pointcloud_container"/>
   <arg name="input/pointcloud_map/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud"/>
-  <arg name="node/pointcloud_container"/>
-  <arg name="use_pointcloud_map"/>
-
-  <!-- Pointcloud filter -->
-  <group>
-    <include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py">
-      <arg name="input_topic" value="$(var input/obstacle_segmentation/pointcloud)"/>
-      <arg name="output_topic" value="pointcloud_map_filtered/pointcloud"/>
-      <arg name="use_intra_process" value="true"/>
-      <arg name="use_multithread" value="true"/>
-      <arg name="pointcloud_container_name" value="$(var node/pointcloud_container)"/>
-      <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
-    </include>
-  </group>
+  <arg name="output/objects"/>
 
   <!-- Clustering -->
   <group>
@@ -43,7 +31,7 @@
     <group>
       <include file="$(find-pkg-share detected_object_feature_remover)/launch/detected_object_feature_remover.launch.xml">
         <arg name="input" value="objects_with_feature"/>
-        <arg name="output" value="objects"/>
+        <arg name="output" value="$(var output/objects)"/>
       </include>
     </group>
   </group>

diff --git a/...ion_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml b/...ion_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml
@@ -1,10 +1,12 @@
 <?xml version="1.0"?>
 <launch>
+  <arg name="output/objects"/>
   <!-- DetectionByTracker -->
   <group>
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml">
       <arg name="detection_by_tracker_param_path" value="$(var object_recognition_detection_detection_by_tracker_param)"/>
+      <arg name="output" value="$(var output/objects)"/>
     </include>
   </group>
 </launch>
diff --git a/...rception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml b/...rception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml
@@ -44,9 +44,9 @@
   <arg name="input/camera7/rois"/>
   <arg name="input/pointcloud_map/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud"/>
-  <arg name="input/lidar_ml/objects" default="$(var lidar_detection_model)/objects"/>
-  <arg name="input/lidar_rule/objects" default="clustering/camera_lidar_fusion/objects"/>
-  <arg name="input/detection_by_tracker/objects" default="detection_by_tracker/objects"/>
+  <arg name="input/lidar_ml/objects"/>
+  <arg name="input/lidar_rule/objects"/>
+  <arg name="input/detection_by_tracker/objects"/>
   <arg name="output/objects" default="objects"/>
 
   <!-- internal interfaces -->

diff --git a/...on_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/...on_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml
@@ -47,16 +47,16 @@
   <arg name="input/camera7/rois"/>
   <arg name="input/pointcloud_map/pointcloud"/>
   <arg name="input/obstacle_segmentation/pointcloud"/>
-  <arg name="input/lidar_ml/objects" default="$(var lidar_detection_model)/objects"/>
-  <arg name="input/lidar_rule/objects" default="clustering/camera_lidar_fusion/objects"/>
-  <arg name="input/radar_near/objects" default="radar/noise_filtered_objects"/>
-  <arg name="input/radar_far/objects" default="radar/far_objects"/>
-  <arg name="input/detection_by_tracker/objects" default="detection_by_tracker/objects"/>
+  <arg name="input/lidar_ml/objects"/>
+  <arg name="input/lidar_rule/objects"/>
+  <arg name="input/radar/objects"/>
+  <arg name="input/radar_far/objects"/>
+  <arg name="input/detection_by_tracker/objects"/>
   <arg name="output/objects" default="objects"/>
 
   <!-- internal interfaces -->
   <let name="radar_objects_fusion/input/objects" value="$(var input/lidar_ml/objects)"/>
-  <let name="radar_objects_fusion/input/radars" value="$(var input/radar_near/objects)"/>
+  <let name="radar_objects_fusion/input/radars" value="$(var input/radar/objects)"/>
   <let name="radar_objects_fusion/output/objects" value="radar_fusion/objects"/>
 
   <let name="camera_roi_fusion/input/objects" value="$(var radar_objects_fusion/output/objects)" if="$(var use_near_radar_fusion)"/>

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
@@ -1,5 +1,8 @@
 <?xml version="1.0"?>
 <launch>
+  <!-- Current namespace -->
+  <let name="ns" value="/perception/object_recognition/tracking"/>
+
   <!-- Radar Tracking and Merger parameters -->
   <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"/>
@@ -13,50 +16,84 @@
   <let name="use_radar_tracking_fusion" value="false" if="$(eval '&quot;$(var mode)&quot;!=&quot;camera_lidar_radar_fusion&quot;')"/>
 
   <!-- External interface -->
-  <arg name="input/detection_input_channels" default="['detected_objects']"/>
-  <!-- detection_input_channels: Input channels for multi object tracker, reference: multi_object_tracker/config/input_channels.param.yaml -->
+  <let name="input/detection_input_channels" value="['detected_objects']"/>
   <arg name="input/radar/objects" default="/perception/object_recognition/detection/radar/far_objects"/>
-  <arg name="output/objects" default="/perception/object_recognition/tracking/objects"/>
-
-  <!-- Internal interface -->
-  <let name="radar_tracker/input/objects" value="$(var input/radar/objects)"/>
-  <let name="radar_tracker/output/objects" value="/perception/object_recognition/tracking/radar/far_objects"/>
-
-  <let name="multi_object_tracker/input/selected_input_channels" value="$(var input/detection_input_channels)"/>
-  <let name="multi_object_tracker/output/objects" value="$(var output/objects)" unless="$(var use_radar_tracking_fusion)"/>
-  <let name="multi_object_tracker/output/objects" value="/perception/object_recognition/tracking/near_objects" if="$(var use_radar_tracking_fusion)"/>
-
-  <let name="tracker_merger/input/main_objects" value="$(var multi_object_tracker/output/objects)"/>
-  <let name="tracker_merger/input/sub_objects" value="$(var radar_tracker/output/objects)"/>
-  <let name="tracker_merger/output/objects" value="$(var output/objects)"/>
-
-  <!--Multi object tracking-->
-  <include file="$(find-pkg-share multi_object_tracker)/launch/multi_object_tracker.launch.xml">
-    <arg name="selected_input_channels" value="$(var multi_object_tracker/input/selected_input_channels)"/>
-    <arg name="output" value="$(var multi_object_tracker/output/objects)"/>
-    <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path)"/>
-    <arg name="input_channels_path" value="$(var object_recognition_tracking_multi_object_tracker_input_channels_param_path)"/>
-    <arg name="tracker_setting_path" value="$(var object_recognition_tracking_multi_object_tracker_node_param_path)"/>
-  </include>
-
-  <!-- Run with tracking merger to add far radar information -->
-  <group if="$(var use_radar_tracking_fusion)">
-    <!--radar long range dynamic object tracking-->
-    <include file="$(find-pkg-share radar_object_tracker)/launch/radar_object_tracker.launch.xml">
-      <arg name="input" value="$(var radar_tracker/input/objects)"/>
-      <arg name="output" value="$(var radar_tracker/output/objects)"/>
-      <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)"/>
-      <arg name="radar_object_tracker_param_path" value="$(var object_recognition_tracking_radar_object_tracker_node_param_path)"/>
+  <arg name="output/objects" default="$(var ns)/objects"/>
+
+  <group unless="$(var use_multi_channel_tracker_merger)">
+    <!-- Internal interface -->
+    <let name="radar_tracker/input/objects" value="$(var input/radar/objects)"/>
+    <let name="radar_tracker/output/objects" value="$(var ns)/radar/far_objects"/>
+
+    <let name="multi_object_tracker/input/selected_input_channels" value="$(var input/detection_input_channels)"/>
+    <let name="multi_object_tracker/output/objects" value="$(var output/objects)" unless="$(var use_radar_tracking_fusion)"/>
+    <let name="multi_object_tracker/output/objects" value="$(var ns)/near_objects" if="$(var use_radar_tracking_fusion)"/>
+
+    <let name="tracker_merger/input/main_objects" value="$(var multi_object_tracker/output/objects)"/>
+    <let name="tracker_merger/input/sub_objects" value="$(var radar_tracker/output/objects)"/>
+    <let name="tracker_merger/output/objects" value="$(var output/objects)"/>
+
+    <!-- Multi object tracking -->
+    <include file="$(find-pkg-share multi_object_tracker)/launch/multi_object_tracker.launch.xml">
+      <arg name="selected_input_channels" value="$(var multi_object_tracker/input/selected_input_channels)"/>
+      <arg name="output" value="$(var multi_object_tracker/output/objects)"/>
+      <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path)"/>
+      <arg name="input_channels_path" value="$(var object_recognition_tracking_multi_object_tracker_input_channels_param_path)"/>
+      <arg name="tracker_setting_path" value="$(var object_recognition_tracking_multi_object_tracker_node_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="$(var tracker_merger/input/main_objects)"/>
-      <arg name="input/sub_object" value="$(var tracker_merger/input/sub_objects)"/>
-      <arg name="output" value="$(var tracker_merger/output/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)"/>
+    <!-- Run with tracking merger to add far radar information -->
+    <group if="$(var use_radar_tracking_fusion)">
+      <!-- radar long range dynamic object tracking -->
+      <include file="$(find-pkg-share radar_object_tracker)/launch/radar_object_tracker.launch.xml">
+        <arg name="input" value="$(var radar_tracker/input/objects)"/>
+        <arg name="output" value="$(var radar_tracker/output/objects)"/>
+        <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)"/>
+        <arg name="radar_object_tracker_param_path" value="$(var object_recognition_tracking_radar_object_tracker_node_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="$(var tracker_merger/input/main_objects)"/>
+        <arg name="input/sub_object" value="$(var tracker_merger/input/sub_objects)"/>
+        <arg name="output" value="$(var tracker_merger/output/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>
+  </group>
+
+  <!-- Object list for multi-channel tracker merger -->
+  <group if="$(var use_multi_channel_tracker_merger)">
+    <let name="detection_enabled_channels" value="$(var input/detection_input_channels)"/>
+    <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_radar_fusion&quot;')">
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','camera_lidar_fusion','detection_by_tracker','radar_far']" if="$(var use_detection_by_tracker)"/>
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','camera_lidar_fusion','radar_far']" unless="$(var use_detection_by_tracker)"/>
+    </group>
+    <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;camera_lidar_fusion&quot;')">
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','camera_lidar_fusion','detection_by_tracker']" if="$(var use_detection_by_tracker)"/>
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','camera_lidar_fusion']" unless="$(var use_detection_by_tracker)"/>
+    </group>
+    <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;lidar_radar_fusion&quot;')">
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','detection_by_tracker','radar_far']" if="$(var use_detection_by_tracker)"/>
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','radar_far']" unless="$(var use_detection_by_tracker)"/>
+    </group>
+    <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;lidar&quot;')">
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated','detection_by_tracker']" if="$(var use_detection_by_tracker)"/>
+      <let name="detection_enabled_channels" value="['lidar_$(var lidar_detection_model)_validated']" unless="$(var use_detection_by_tracker)"/>
+    </group>
+    <group scoped="false" if="$(eval '&quot;$(var mode)&quot;==&quot;radar&quot;')">
+      <let name="detection_enabled_channels" value="['radar_far']"/>
+    </group>
+
+    <!-- Multi object tracking -->
+    <include file="$(find-pkg-share multi_object_tracker)/launch/multi_object_tracker.launch.xml">
+      <arg name="selected_input_channels" value="$(var detection_enabled_channels)"/>
+      <arg name="output" value="$(var output/objects)"/>
+      <arg name="data_association_matrix_path" value="$(var object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path)"/>
+      <arg name="input_channels_path" value="$(var object_recognition_tracking_multi_object_tracker_input_channels_param_path)"/>
+      <arg name="tracker_setting_path" value="$(var object_recognition_tracking_multi_object_tracker_node_param_path)"/>
     </include>
   </group>
 </launch>