feat(multi_object_tracker): process modulation (autowarefoundation#6742)

* fix: reduce lower limit of publish interval Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: enlarge publish range upper limit Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: set parameter tested and agreed Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: define functions on each tracking steps Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: remove time argument to avoid misuse Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: refine function inputs and outputs Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: create tracking process class Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: clean up Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: move processor to subfolder Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: output filter is moved to the processor side Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: rename tracker management process, process parameter Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: revise comments Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: remove not-related dependency Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
Interplai · Jun 3, 2024 · b2d8422 · b2d8422
1 parent b9c1127
commit b2d8422
Show file tree

Hide file tree

Showing 7 changed files with 414 additions and 238 deletions.
diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt
@@ -23,6 +23,7 @@ include_directories(
 set(MULTI_OBJECT_TRACKER_SRC
   src/multi_object_tracker_core.cpp
   src/debugger.cpp
+  src/processor/processor.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/include/multi_object_tracker/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp
@@ -56,6 +56,7 @@ class TrackerDebugger
   } debug_settings_;
   double pipeline_latency_ms_ = 0.0;
   diagnostic_updater::Updater diagnostic_updater_;
+  bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; }
 
 private:
   void loadParameters();

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp
@@ -21,6 +21,7 @@
 
 #include "multi_object_tracker/data_association/data_association.hpp"
 #include "multi_object_tracker/debugger.hpp"
+#include "multi_object_tracker/processor/processor.hpp"
 #include "multi_object_tracker/tracker/model/tracker_base.hpp"
 
 #include <rclcpp/rclcpp.hpp>
@@ -46,6 +47,7 @@
 #include <map>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
 class MultiObjectTracker : public rclcpp::Node
@@ -54,42 +56,34 @@ class MultiObjectTracker : public rclcpp::Node
   explicit MultiObjectTracker(const rclcpp::NodeOptions & node_options);
 
 private:
+  // ROS interface
   rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
     tracked_objects_pub_;
   rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
     detected_object_sub_;
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  // debugger
+  std::unique_ptr<TrackerDebugger> debugger_;
+  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 
   // publish timer
   rclcpp::TimerBase::SharedPtr publish_timer_;
   rclcpp::Time last_published_time_;
   double publisher_period_;
 
-  // debugger class
-  std::unique_ptr<TrackerDebugger> debugger_;
-
-  tf2_ros::Buffer tf_buffer_;
-  tf2_ros::TransformListener tf_listener_;
-
-  std::map<std::uint8_t, std::string> tracker_map_;
-
-  std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
+  // internal states
+  std::string world_frame_id_;  // tracking frame
+  std::unique_ptr<DataAssociation> data_association_;
+  std::unique_ptr<TrackerProcessor> processor_;
 
+  // callback functions
   void onMeasurement(
     const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg);
   void onTimer();
 
-  std::string world_frame_id_;  // tracking frame
-  std::list<std::shared_ptr<Tracker>> list_tracker_;
-  std::unique_ptr<DataAssociation> data_association_;
-
-  void checkTrackerLifeCycle(
-    std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time);
-  void sanitizeTracker(
-    std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time);
-  std::shared_ptr<Tracker> createNewTracker(
-    const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform) const;
-
+  // publish processes
   void checkAndPublish(const rclcpp::Time & time);
   void publish(const rclcpp::Time & time) const;
   inline bool shouldTrackerPublish(const std::shared_ptr<const Tracker> tracker) const;

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp
@@ -0,0 +1,79 @@
+// 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__PROCESSOR__PROCESSOR_HPP_
+#define MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_
+
+#include "multi_object_tracker/tracker/model/tracker_base.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
+
+#include <list>
+#include <map>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
+class TrackerProcessor
+{
+public:
+  explicit TrackerProcessor(const std::map<std::uint8_t, std::string> & tracker_map);
+
+  const std::list<std::shared_ptr<Tracker>> & getListTracker() const { return list_tracker_; }
+  // tracker processes
+  void predict(const rclcpp::Time & time);
+  void update(
+    const autoware_auto_perception_msgs::msg::DetectedObjects & transformed_objects,
+    const geometry_msgs::msg::Transform & self_transform,
+    const std::unordered_map<int, int> & direct_assignment);
+  void spawn(
+    const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects,
+    const geometry_msgs::msg::Transform & self_transform,
+    const std::unordered_map<int, int> & reverse_assignment);
+  void prune(const rclcpp::Time & time);
+
+  // output
+  bool isConfidentTracker(const std::shared_ptr<Tracker> & tracker) const;
+  void getTrackedObjects(
+    const rclcpp::Time & time,
+    autoware_auto_perception_msgs::msg::TrackedObjects & tracked_objects) const;
+  void getTentativeObjects(
+    const rclcpp::Time & time,
+    autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const;
+
+private:
+  std::map<std::uint8_t, std::string> tracker_map_;
+  std::list<std::shared_ptr<Tracker>> list_tracker_;
+
+  // parameters
+  float max_elapsed_time_;            // [s]
+  float min_iou_;                     // [ratio]
+  float min_iou_for_unknown_object_;  // [ratio]
+  double distance_threshold_;         // [m]
+  int confident_count_threshold_;     // [count]
+
+  void removeOldTracker(const rclcpp::Time & time);
+  void removeOverlappedTracker(const rclcpp::Time & time);
+  std::shared_ptr<Tracker> createNewTracker(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
+    const geometry_msgs::msg::Transform & self_transform) const;
+};
+
+#endif  // MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_
diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml
@@ -25,7 +25,6 @@
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>
-  <depend>tier4_perception_msgs</depend>
   <depend>unique_identifier_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>