feat(multi_object_tracker): multi object input (#6820)

* refactor: frequently used types, namespace Signed-off-by: Taekjin LEE <[email protected]> * test: multiple inputs Signed-off-by: Taekjin LEE <[email protected]> * feat: check latest measurement time Signed-off-by: Taekjin LEE <[email protected]> * feat: define input manager class Signed-off-by: Taekjin LEE <[email protected]> * feat: interval measures Signed-off-by: Taekjin LEE <[email protected]> * feat: store and sort inputs PoC Signed-off-by: Taekjin LEE <[email protected]> * chore: rename classes Signed-off-by: Taekjin LEE <[email protected]> * feat: object collector Signed-off-by: Taekjin LEE <[email protected]> * impl input manager, no subscribe Signed-off-by: Taekjin LEE <[email protected]> * fix: subscribe and trigger callback Signed-off-by: Taekjin LEE <[email protected]> * fix: subscriber and callbacks are working Signed-off-by: Taekjin LEE <[email protected]> * fix: callback object is fixed, tracker is working Signed-off-by: Taekjin LEE <[email protected]> * fix: get object time argument revise Signed-off-by: Taekjin LEE <[email protected]> * feat: back to periodic publish, analyze input latency and timings Signed-off-by: Taekjin LEE <[email protected]> * fix: enable timing debugger Signed-off-by: Taekjin LEE <[email protected]> * fix: separate object interval function Signed-off-by: Taekjin LEE <[email protected]> * feat: prepare message triggered process Signed-off-by: Taekjin LEE <[email protected]> * feat: trigger tracker by main message arrive Signed-off-by: Taekjin LEE <[email protected]> * chore: clean-up, set namespace Signed-off-by: Taekjin LEE <[email protected]> * feat: object lists with detector index Signed-off-by: Taekjin LEE <[email protected]> * feat: define input channel struct Signed-off-by: Taekjin LEE <[email protected]> * fix: define type for object list Signed-off-by: Taekjin LEE <[email protected]> * feat: add channel wise existence probability Signed-off-by: Taekjin LEE <[email protected]> * fix: relocate debugger Signed-off-by: Taekjin LEE <[email protected]> * fix: total existence logic change Signed-off-by: Taekjin LEE <[email protected]> * feat: publishing object debug info, need to fix marker id Signed-off-by: Taekjin LEE <[email protected]> * feat: indexing marker step 1 Signed-off-by: Taekjin LEE <[email protected]> * fix: uuid management Signed-off-by: Taekjin LEE <[email protected]> * feat: association line fix Signed-off-by: Taekjin LEE <[email protected]> * feat: print channel names Signed-off-by: Taekjin LEE <[email protected]> * feat: association lines are color-coded Signed-off-by: Taekjin LEE <[email protected]> * fix: association debug marker bugfix Signed-off-by: Taekjin LEE <[email protected]> * style(pre-commit): autofix Signed-off-by: Taekjin LEE <[email protected]> * feat: add option for debug marker Signed-off-by: Taekjin LEE <[email protected]> * feat: skip time statistics update in case of outlier Signed-off-by: Taekjin LEE <[email protected]> * feat: auto-tune latency band Signed-off-by: Taekjin LEE <[email protected]> * feat: pre-defined channels, select on launcher Signed-off-by: Taekjin LEE <[email protected]> * feat: add input channels Signed-off-by: Taekjin LEE <[email protected]> * fix: remove marker idx map Signed-off-by: Taekjin LEE <[email protected]> * fix: to do not miss the latest message of the target stream Signed-off-by: Taekjin LEE <[email protected]> * fix: remove priority, separate timing optimization Signed-off-by: Taekjin LEE <[email protected]> * fix: time interval bug fix Signed-off-by: Taekjin LEE <[email protected]> * chore: refactoring timing state update Signed-off-by: Taekjin LEE <[email protected]> * fix: set parameters optionally Signed-off-by: Taekjin LEE <[email protected]> * feat: revise object time range logic Signed-off-by: Taekjin LEE <[email protected]> * fix: launcher to set input channels Signed-off-by: Taekjin LEE <[email protected]> * fix: exempt spell check 'pointpainting' Signed-off-by: Taekjin LEE <[email protected]> * feat: remove expected interval Signed-off-by: Taekjin LEE <[email protected]> * feat: implement spawn switch Signed-off-by: Taekjin LEE <[email protected]> * fix: remove debug messages Signed-off-by: Taekjin LEE <[email protected]> * chore: update readme Signed-off-by: Taekjin LEE <[email protected]> * fix: change tentative object topic Signed-off-by: Taekjin LEE <[email protected]> * Revert "fix: remove debug messages" This reverts commit 725a49e. Signed-off-by: Taekjin LEE <[email protected]> * fix: reset times when jumps to past Signed-off-by: Taekjin LEE <[email protected]> * fix: check if interval is negative Signed-off-by: Taekjin LEE <[email protected]> * fix: missing config, default value Signed-off-by: Taekjin LEE <[email protected]> * fix: remove debug messages Signed-off-by: Taekjin LEE <[email protected]> * fix: change no-object message level Signed-off-by: Taekjin LEE <[email protected]> * Update perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp Co-authored-by: Shunsuke Miura <[email protected]> Signed-off-by: Taekjin LEE <[email protected]> * chore: Update copyright to uppercase Signed-off-by: Taekjin LEE <[email protected]> * chore: fix readme links to config files Signed-off-by: Taekjin LEE <[email protected]> * chore: move and rename uuid functions Signed-off-by: Taekjin LEE <[email protected]> * chore: fix debug topic to use node name Signed-off-by: Taekjin LEE <[email protected]> * chore: express meaning of threshold Signed-off-by: Taekjin LEE <[email protected]> * feat: revise decay rate, update function Signed-off-by: Taekjin LEE <[email protected]> * fix: define constants with explanation Signed-off-by: Taekjin LEE <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: Taekjin LEE <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shunsuke Miura <[email protected]>
autowarefoundation · May 23, 2024 · 3aa84b0 · 3aa84b0
1 parent 7877192
commit 3aa84b0
Show file tree

Hide file tree

Showing 36 changed files with 1,616 additions and 205 deletions.
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
@@ -17,6 +17,7 @@
     <!--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="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>
@@ -26,6 +27,7 @@
     <!--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="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)"/>
       <arg name="output" value="/perception/object_recognition/tracking/near_objects"/>
     </include>

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -22,6 +22,7 @@
   <arg name="object_recognition_detection_object_range_splitter_radar_param_path"/>
   <arg name="object_recognition_detection_object_range_splitter_radar_fusion_param_path"/>
   <arg name="object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path"/>
+  <arg name="object_recognition_tracking_multi_object_tracker_input_channels_param_path"/>
   <arg name="object_recognition_tracking_multi_object_tracker_node_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"/>

diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt
@@ -22,8 +22,10 @@ include_directories(
 # Generate exe file
 set(MULTI_OBJECT_TRACKER_SRC
   src/multi_object_tracker_core.cpp
-  src/debugger.cpp
+  src/debugger/debugger.cpp
+  src/debugger/debug_object.cpp
   src/processor/processor.cpp
+  src/processor/input_manager.cpp
   src/data_association/data_association.cpp
   src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
   src/tracker/motion_model/motion_model_base.cpp

diff --git a/perception/multi_object_tracker/README.md b/perception/multi_object_tracker/README.md
@@ -46,31 +46,38 @@ Example:
 
 ### Input
 
-| Name      | Type                                                  | Description |
-| --------- | ----------------------------------------------------- | ----------- |
-| `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | obstacles   |
+Multiple inputs are pre-defined in the input channel parameters (described below) and the inputs can be configured
+
+| Name                      | Type                       | Description            |
+| ------------------------- | -------------------------- | ---------------------- |
+| `selected_input_channels` | `std::vector<std::string>` | array of channel names |
+
+- default value: `selected_input_channels:="['detected_objects']"`, merged DetectedObject message
+- multi-input example: `selected_input_channels:="['lidar_centerpoint','camera_lidar_fusion','detection_by_tracker','radar_far']"`
 
 ### Output
 
-| Name       | Type                                                 | Description        |
-| ---------- | ---------------------------------------------------- | ------------------ |
-| `~/output` | `autoware_auto_perception_msgs::msg::TrackedObjects` | modified obstacles |
+| Name       | Type                                                 | Description     |
+| ---------- | ---------------------------------------------------- | --------------- |
+| `~/output` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracked objects |
 
 ## Parameters
 
-<!-- Write parameters of this package.
+### Input Channel parameters
 
-Example:
-  ### Node Parameters
+Available input channels are defined in [input_channels.param.yaml](config/input_channels.param.yaml).
 
-  | Name                   | Type | Description                     |
-  | ---------------------- | ---- | ------------------------------- |
-  | `output_debug_markers` | bool | whether to output debug markers |
--->
+| Name                              | Type                                                  | Description                           |
+| --------------------------------- | ----------------------------------------------------- | ------------------------------------- |
+| `<channel>`                       |                                                       | the name of channel                   |
+| `<channel>.topic`                 | `autoware_auto_perception_msgs::msg::DetectedObjects` | detected objects                      |
+| `<channel>.can_spawn_new_tracker` | `bool`                                                | a switch allow to spawn a new tracker |
+| `<channel>.optional.name`         | `std::string`                                         | channel name for analysis             |
+| `<channel>.optional.short_name`   | `std::string`                                         | short name for visualization          |
 
 ### Core Parameters
 
-Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_object_tracker.param.yaml) and association parameters are defined in [data_association.param.yaml](config/data_association.param.yaml).
+Node parameters are defined in [multi_object_tracker_node.param.yaml](config/multi_object_tracker_node.param.yaml) and association parameters are defined in [data_association_matrix.param.yaml](config/data_association_matrix.param.yaml).
 
 #### Node parameters
 
@@ -80,6 +87,9 @@ Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_ob
 | `world_frame_id`            | double | object kinematics definition frame                                                                                          |
 | `enable_delay_compensation` | bool   | if True, tracker use timers to schedule publishers and use prediction step to extrapolate object state at desired timestamp |
 | `publish_rate`              | double | Timer frequency to output with delay compensation                                                                           |
+| `publish_processing_time`   | bool   | enable to publish debug message of process time information                                                                 |
+| `publish_tentative_objects` | bool   | enable to publish tentative tracked objects, which have lower confidence                                                    |
+| `publish_debug_markers`     | bool   | enable to publish debug markers, which indicates association of multi-inputs, existence probability of each detection       |
 
 #### Association parameters
 
@@ -93,13 +103,6 @@ Node parameters are defined in [multi_object_tracker.param.yaml](config/multi_ob
 
 ## Assumptions / Known limits
 
-<!-- Write assumptions and limitations of your implementation.
-
-Example:
-  This algorithm assumes obstacles are not moving, so if they rapidly move after the vehicle started to avoid them, it might collide with them.
-  Also, this algorithm doesn't care about blind spots. In general, since too close obstacles aren't visible due to the sensing performance limit, please take enough margin to obstacles.
--->
-
 See the [model explanations](models.md).
 
 ## (Optional) Error detection and handling

diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml
@@ -0,0 +1,82 @@
+/**:
+  ros__parameters:
+    input_channels:
+      detected_objects:
+        topic: "/perception/object_recognition/detection/objects"
+        can_spawn_new_tracker: true
+        optional:
+          name: "detected_objects"
+          short_name: "all"
+      # LIDAR - rule-based
+      lidar_clustering:
+        topic: "/perception/object_recognition/detection/clustering/objects"
+        can_spawn_new_tracker: true
+        optional:
+          name: "clustering"
+          short_name: "Lcl"
+      # LIDAR - DNN
+      lidar_centerpoint:
+        topic: "/perception/object_recognition/detection/centerpoint/objects"
+        can_spawn_new_tracker: true
+        optional:
+          name: "centerpoint"
+          short_name: "Lcp"
+      lidar_centerpoint_validated:
+        topic: "/perception/object_recognition/detection/centerpoint/validation/objects"
+        can_spawn_new_tracker: true
+        optional:
+          name: "centerpoint"
+          short_name: "Lcp"
+      lidar_apollo:
+        topic: "/perception/object_recognition/detection/apollo/objects"
+        can_spawn_new_tracker: true
+        optional:
+          name: "apollo"
+          short_name: "Lap"
+      lidar_apollo_validated:
+        topic: "/perception/object_recognition/detection/apollo/validation/objects"
+        can_spawn_new_tracker: true
+        optional:
+          name: "apollo"
+          short_name: "Lap"
+      # LIDAR-CAMERA - DNN
+      # cspell:ignore lidar_pointpainitng pointpainting
+      lidar_pointpainitng:
+        topic: "/perception/object_recognition/detection/pointpainting/objects"
+        can_spawn_new_tracker: true
+        optional:
+          name: "pointpainting"
+          short_name: "Lpp"
+      lidar_pointpainting_validated:
+        topic: "/perception/object_recognition/detection/pointpainting/validation/objects"
+        can_spawn_new_tracker: true
+        optional:
+          name: "pointpainting"
+          short_name: "Lpp"
+      # CAMERA-LIDAR
+      camera_lidar_fusion:
+        topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects"
+        can_spawn_new_tracker: true
+        optional:
+          name: "camera_lidar_fusion"
+          short_name: "CLf"
+      # CAMERA-LIDAR+TRACKER
+      detection_by_tracker:
+        topic: "/perception/object_recognition/detection/detection_by_tracker/objects"
+        can_spawn_new_tracker: false
+        optional:
+          name: "detection_by_tracker"
+          short_name: "dbT"
+      # RADAR
+      radar:
+        topic: "/sensing/radar/detected_objects"
+        can_spawn_new_tracker: true
+        optional:
+          name: "radar"
+          short_name: "R"
+      radar_far:
+        topic: "/perception/object_recognition/detection/radar/far_objects"
+        can_spawn_new_tracker: true
+        optional:
+          name: "radar_far"
+          short_name: "Rf"
diff --git a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml
@@ -17,5 +17,6 @@
     # debug parameters
     publish_processing_time: false
     publish_tentative_objects: false
+    publish_debug_markers: false
     diagnostics_warn_delay: 0.5      # [sec]
     diagnostics_error_delay: 1.0     # [sec]
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp
@@ -0,0 +1,102 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_
+#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_
+
+#include "multi_object_tracker/tracker/model/tracker_base.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/uuid_helper.hpp>
+
+#include "unique_identifier_msgs/msg/uuid.hpp"
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
+#include <geometry_msgs/msg/point.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include <boost/functional/hash.hpp>
+#include <boost/uuid/uuid.hpp>
+#include <boost/uuid/uuid_generators.hpp>
+
+#include <list>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <unordered_set>
+#include <vector>
+
+struct ObjectData
+{
+  rclcpp::Time time;
+
+  // object uuid
+  boost::uuids::uuid uuid;
+  int uuid_int;
+  std::string uuid_str;
+
+  // association link, pair of coordinates
+  // tracker to detection
+  geometry_msgs::msg::Point tracker_point;
+  geometry_msgs::msg::Point detection_point;
+  bool is_associated{false};
+
+  // existence probabilities
+  std::vector<float> existence_vector;
+
+  // detection channel id
+  uint channel_id;
+};
+
+class TrackerObjectDebugger
+{
+public:
+  explicit TrackerObjectDebugger(std::string frame_id);
+
+private:
+  bool is_initialized_{false};
+  std::string frame_id_;
+  visualization_msgs::msg::MarkerArray markers_;
+  std::unordered_set<int> current_ids_;
+  std::unordered_set<int> previous_ids_;
+  rclcpp::Time message_time_;
+
+  std::vector<ObjectData> object_data_list_;
+  std::list<int32_t> unused_marker_ids_;
+  int32_t marker_id_ = 0;
+  std::vector<std::vector<ObjectData>> object_data_groups_;
+
+  std::vector<std::string> channel_names_;
+
+public:
+  void setChannelNames(const std::vector<std::string> & channel_names)
+  {
+    channel_names_ = channel_names;
+  }
+  void collect(
+    const rclcpp::Time & message_time, const std::list<std::shared_ptr<Tracker>> & list_tracker,
+    const uint & channel_index,
+    const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects,
+    const std::unordered_map<int, int> & direct_assignment,
+    const std::unordered_map<int, int> & reverse_assignment);
+
+  void reset();
+  void draw(
+    const std::vector<std::vector<ObjectData>> object_data_groups,
+    visualization_msgs::msg::MarkerArray & marker_array) const;
+  void process();
+  void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const;
+};
+
+#endif  // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_