fix(multi_object_tracker): huge tracking object bug fix for x2

perception/multi_object_tracker/CMakeLists.txt

@@ -12,6 +12,7 @@ endif()
 ### Find Eigen Dependencies
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
+find_package(glog REQUIRED)
 
 include_directories(
   SYSTEM
@@ -21,6 +22,15 @@ include_directories(
 # Generate exe file
 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
+  src/tracker/motion_model/bicycle_motion_model.cpp
+  # cspell: ignore ctrv
+  src/tracker/motion_model/ctrv_motion_model.cpp
+  src/tracker/motion_model/cv_motion_model.cpp
   src/tracker/model/tracker_base.cpp
   src/tracker/model/big_vehicle_tracker.cpp
   src/tracker/model/normal_vehicle_tracker.cpp
@@ -30,21 +40,20 @@ set(MULTI_OBJECT_TRACKER_SRC
   src/tracker/model/pedestrian_and_bicycle_tracker.cpp
   src/tracker/model/unknown_tracker.cpp
   src/tracker/model/pass_through_tracker.cpp
-  src/data_association/data_association.cpp
-  src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
 )
 
-ament_auto_add_library(multi_object_tracker_node SHARED
+ament_auto_add_library(${PROJECT_NAME} SHARED
   ${MULTI_OBJECT_TRACKER_SRC}
 )
 
-target_link_libraries(multi_object_tracker_node
+target_link_libraries(${PROJECT_NAME}
   Eigen3::Eigen
+  glog::glog
 )
 
-rclcpp_components_register_node(multi_object_tracker_node
+rclcpp_components_register_node(${PROJECT_NAME}
   PLUGIN "MultiObjectTracker"
-  EXECUTABLE multi_object_tracker
+  EXECUTABLE ${PROJECT_NAME}_node
 )
 
 ament_auto_package(INSTALL_TO_SHARE

perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp

@@ -0,0 +1,75 @@
+// 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_HPP_
+#define MULTI_OBJECT_TRACKER__DEBUGGER_HPP_
+
+#include <diagnostic_updater/diagnostic_updater.hpp>
+#include <diagnostic_updater/publisher.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/ros/debug_publisher.hpp>
+// #include <tier4_autoware_utils/ros/published_time_publisher.hpp>
+
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
+#include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+
+#include <memory>
+
+/**
+ * @brief Debugger class for multi object tracker
+ * @details This class is used to publish debug information of multi object tracker
+ */
+class TrackerDebugger
+{
+public:
+  explicit TrackerDebugger(rclcpp::Node & node);
+  void publishTentativeObjects(
+    const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const;
+  void startMeasurementTime(
+    const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp);
+  void endMeasurementTime(const rclcpp::Time & now);
+  void startPublishTime(const rclcpp::Time & now);
+  void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time);
+
+  void setupDiagnostics();
+  void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat);
+  struct DEBUG_SETTINGS
+  {
+    bool publish_processing_time;
+    bool publish_tentative_objects;
+    double diagnostics_warn_delay;
+    double diagnostics_error_delay;
+  } 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();
+  bool is_initialized_ = false;
+  rclcpp::Node & node_;
+  rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
+    debug_tentative_objects_pub_;
+  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
+  rclcpp::Time last_input_stamp_;
+  rclcpp::Time stamp_process_start_;
+  rclcpp::Time stamp_process_end_;
+  rclcpp::Time stamp_publish_start_;
+  rclcpp::Time stamp_publish_output_;
+};
+
+#endif  // MULTI_OBJECT_TRACKER__DEBUGGER_HPP_

perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp

@@ -20,11 +20,11 @@
 #define MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_
 
 #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>
-#include <tier4_autoware_utils/ros/debug_publisher.hpp>
-#include <tier4_autoware_utils/system/stop_watch.hpp>
 
 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
@@ -40,91 +40,52 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
-#include <diagnostic_updater/diagnostic_updater.hpp>
-#include <diagnostic_updater/publisher.hpp>
-
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
 #include <list>
 #include <map>
 #include <memory>
 #include <string>
+#include <unordered_map>
 #include <vector>
 
-/**
- * @brief Debugger class for multi object tracker
- * @details This class is used to publish debug information of multi object tracker
- */
-class TrackerDebugger
-{
-public:
-  explicit TrackerDebugger(rclcpp::Node & node);
-  void publishProcessingTime();
-  void publishTentativeObjects(
-    const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const;
-  void startStopWatch();
-  void startMeasurementTime(const rclcpp::Time & measurement_header_stamp);
-  void setupDiagnostics();
-  void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat);
-  struct DEBUG_SETTINGS
-  {
-    bool publish_processing_time;
-    bool publish_tentative_objects;
-    double diagnostics_warn_delay;
-    double diagnostics_error_delay;
-  } debug_settings_;
-  double elapsed_time_from_sensor_input_ = 0.0;
-  diagnostic_updater::Updater diagnostic_updater_;
-
-private:
-  void loadParameters();
-  rclcpp::Node & node_;
-  rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr
-    debug_tentative_objects_pub_;
-  std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
-  std::unique_ptr<tier4_autoware_utils::DebugPublisher> processing_time_publisher_;
-  rclcpp::Time last_input_stamp_;
-};
-
 class MultiObjectTracker : public rclcpp::Node
 {
 public:
   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_;
-  rclcpp::TimerBase::SharedPtr publish_timer_;  // publish timer
-
-  // 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_;
+  // debugger
+  std::unique_ptr<TrackerDebugger> debugger_;
+  // std::unique_ptr<tier4_autoware_utils::PublishedTimePublisher> published_time_publisher_;
 
-  void onMeasurement(
-    const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg);
-  void onTimer();
+  // publish timer
+  rclcpp::TimerBase::SharedPtr publish_timer_;
+  rclcpp::Time last_published_time_;
+  double publisher_period_;
 
+  // internal states
   std::string world_frame_id_;  // tracking frame
-  std::list<std::shared_ptr<Tracker>> list_tracker_;
   std::unique_ptr<DataAssociation> data_association_;
+  std::unique_ptr<TrackerProcessor> processor_;
 
-  void checkTrackerLifeCycle(
-    std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform);
-  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;
+  // callback functions
+  void onMeasurement(
+    const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg);
+  void onTimer();
 
-  void publish(const rclcpp::Time & time);
+  // 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;
 };
 

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_