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

autowarefoundation#1314)"

This reverts commit 4b1f18e.

perception/multi_object_tracker/CMakeLists.txt

@@ -12,7 +12,6 @@ endif()
 ### Find Eigen Dependencies
 find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
-find_package(glog REQUIRED)
 
 include_directories(
   SYSTEM
@@ -22,15 +21,6 @@ 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
@@ -40,20 +30,21 @@ 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(${PROJECT_NAME} SHARED
+ament_auto_add_library(multi_object_tracker_node SHARED
   ${MULTI_OBJECT_TRACKER_SRC}
 )
 
-target_link_libraries(${PROJECT_NAME}
+target_link_libraries(multi_object_tracker_node
   Eigen3::Eigen
-  glog::glog
 )
 
-rclcpp_components_register_node(${PROJECT_NAME}
+rclcpp_components_register_node(multi_object_tracker_node
   PLUGIN "MultiObjectTracker"
-  EXECUTABLE ${PROJECT_NAME}_node
+  EXECUTABLE multi_object_tracker
 )
 
 ament_auto_package(INSTALL_TO_SHARE

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,52 +40,91 @@
 #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_;
-  tf2_ros::Buffer tf_buffer_;
-  tf2_ros::TransformListener tf_listener_;
+  rclcpp::TimerBase::SharedPtr publish_timer_;  // publish timer
 
-  // debugger
+  // debugger class
   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_;
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
 
-  // internal states
-  std::string world_frame_id_;  // tracking frame
-  std::unique_ptr<DataAssociation> data_association_;
-  std::unique_ptr<TrackerProcessor> processor_;
+  std::map<std::uint8_t, std::string> tracker_map_;
 
-  // callback functions
   void onMeasurement(
     const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg);
   void onTimer();
 
-  // publish processes
-  void checkAndPublish(const rclcpp::Time & time);
-  void publish(const rclcpp::Time & time) const;
+  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,
+    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;
+
+  void publish(const rclcpp::Time & time);
   inline bool shouldTrackerPublish(const std::shared_ptr<const Tracker> tracker) const;
 };