From 5412a55041365af139783349692c9e738e5c83ff Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 11 Apr 2024 17:03:16 +0900
Subject: [PATCH 01/67] refactor: frequently used types, namespace

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_core.hpp             | 18 +++++++++-----
 .../src/multi_object_tracker_core.cpp         | 24 ++++++++++---------
 .../src/multi_object_tracker_node.cpp         |  2 +-
 3 files changed, 26 insertions(+), 18 deletions(-)

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
index f788dd3895216..483eb38bf2697 100644
--- 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
@@ -50,6 +50,13 @@
 #include <unordered_map>
 #include <vector>
 
+namespace multi_object_tracker
+{
+
+using autoware_auto_perception_msgs::msg::DetectedObject;
+using autoware_auto_perception_msgs::msg::DetectedObjects;
+using autoware_auto_perception_msgs::msg::TrackedObjects;
+
 class MultiObjectTracker : public rclcpp::Node
 {
 public:
@@ -57,10 +64,8 @@ class MultiObjectTracker : public rclcpp::Node
 
 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::Publisher<TrackedObjects>::SharedPtr tracked_objects_pub_;
+  rclcpp::Subscription<DetectedObjects>::SharedPtr detected_object_sub_;
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
 
@@ -79,8 +84,7 @@ class MultiObjectTracker : public rclcpp::Node
   std::unique_ptr<TrackerProcessor> processor_;
 
   // callback functions
-  void onMeasurement(
-    const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg);
+  void onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg);
   void onTimer();
 
   // publish processes
@@ -89,4 +93,6 @@ class MultiObjectTracker : public rclcpp::Node
   inline bool shouldTrackerPublish(const std::shared_ptr<const Tracker> tracker) const;
 };
 
+}  // namespace multi_object_tracker
+
 #endif  // MULTI_OBJECT_TRACKER__MULTI_OBJECT_TRACKER_CORE_HPP_
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index d80a21813b28b..6f343dd5de8fb 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -36,8 +36,6 @@
 #include <Eigen/Geometry>
 #include <rclcpp_components/register_node_macro.hpp>
 
-using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
-
 namespace
 {
 // Function to get the transform between two frames
@@ -67,6 +65,10 @@ boost::optional<geometry_msgs::msg::Transform> getTransformAnonymous(
 
 }  // namespace
 
+namespace multi_object_tracker
+{
+using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
+
 MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
 : rclcpp::Node("multi_object_tracker", node_options),
   tf_buffer_(this->get_clock()),
@@ -74,11 +76,10 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   last_published_time_(this->now())
 {
   // Create publishers and subscribers
-  detected_object_sub_ = create_subscription<autoware_auto_perception_msgs::msg::DetectedObjects>(
+  detected_object_sub_ = create_subscription<DetectedObjects>(
     "input", rclcpp::QoS{1},
     std::bind(&MultiObjectTracker::onMeasurement, this, std::placeholders::_1));
-  tracked_objects_pub_ =
-    create_publisher<autoware_auto_perception_msgs::msg::TrackedObjects>("output", rclcpp::QoS{1});
+  tracked_objects_pub_ = create_publisher<TrackedObjects>("output", rclcpp::QoS{1});
 
   // Get parameters
   double publish_rate = declare_parameter<double>("publish_rate");  // [hz]
@@ -142,8 +143,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
-void MultiObjectTracker::onMeasurement(
-  const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg)
+void MultiObjectTracker::onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg)
 {
   // Get the time of the measurement
   const rclcpp::Time measurement_time =
@@ -157,7 +157,7 @@ void MultiObjectTracker::onMeasurement(
     return;
   }
   /* transform to world coordinate */
-  autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects;
+  DetectedObjects transformed_objects;
   if (!object_recognition_utils::transformObjects(
         *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) {
     return;
@@ -234,7 +234,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const
     return;
   }
   // Create output msg
-  autoware_auto_perception_msgs::msg::TrackedObjects output_msg, tentative_objects_msg;
+  TrackedObjects output_msg, tentative_objects_msg;
   output_msg.header.frame_id = world_frame_id_;
   processor_->getTrackedObjects(time, output_msg);
 
@@ -246,11 +246,13 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const
   debugger_->endPublishTime(this->now(), time);
 
   if (debugger_->shouldPublishTentativeObjects()) {
-    autoware_auto_perception_msgs::msg::TrackedObjects tentative_objects_msg;
+    TrackedObjects tentative_objects_msg;
     tentative_objects_msg.header.frame_id = world_frame_id_;
     processor_->getTentativeObjects(time, tentative_objects_msg);
     debugger_->publishTentativeObjects(tentative_objects_msg);
   }
 }
 
-RCLCPP_COMPONENTS_REGISTER_NODE(MultiObjectTracker)
+}  // namespace multi_object_tracker
+
+RCLCPP_COMPONENTS_REGISTER_NODE(multi_object_tracker::MultiObjectTracker)
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp
index c6eed297c02c4..273e6ac1a06bd 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_node.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_node.cpp
@@ -27,7 +27,7 @@ int main(int argc, char ** argv)
 
   rclcpp::init(argc, argv);
   rclcpp::NodeOptions options;
-  auto multi_object_tracker = std::make_shared<MultiObjectTracker>(options);
+  auto multi_object_tracker = std::make_shared<multi_object_tracker::MultiObjectTracker>(options);
   rclcpp::executors::SingleThreadedExecutor exec;
   exec.add_node(multi_object_tracker);
   exec.spin();

From 4176cec169e2a63d1a933668e8dff9e2c9ae5aa1 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 11 Apr 2024 17:49:48 +0900
Subject: [PATCH 02/67] test: multiple inputs

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_node.param.yaml      |  2 ++
 .../multi_object_tracker_core.hpp             |  9 +++++
 .../src/multi_object_tracker_core.cpp         | 35 +++++++++++++++++--
 3 files changed, 43 insertions(+), 3 deletions(-)

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
index ca320c7f58442..09b9adee8caa8 100644
--- a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml
+++ b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml
@@ -19,3 +19,5 @@
     publish_tentative_objects: false
     diagnostics_warn_delay: 0.5      # [sec]
     diagnostics_error_delay: 1.0     # [sec]
+
+    input_topic_names: ["/perception/object_recognition/detection/objects"]
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
index 483eb38bf2697..e621479d4e886 100644
--- 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
@@ -83,7 +83,16 @@ class MultiObjectTracker : public rclcpp::Node
   std::unique_ptr<DataAssociation> data_association_;
   std::unique_ptr<TrackerProcessor> processor_;
 
+  // inputs
+  rclcpp::Subscription<DetectedObjects>::SharedPtr sub_objects_{};
+  std::vector<rclcpp::Subscription<DetectedObjects>::SharedPtr> sub_objects_array_{};
+
+  std::vector<std::string> input_topic_names_{};
+  size_t input_topic_size_{};
+  std::vector<DetectedObjects::ConstSharedPtr> objects_data_{};
+
   // callback functions
+  void onData(DetectedObjects::ConstSharedPtr msg, const size_t array_number);
   void onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg);
   void onTimer();
 
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 6f343dd5de8fb..e55b0d0371c55 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -76,15 +76,35 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   last_published_time_(this->now())
 {
   // Create publishers and subscribers
-  detected_object_sub_ = create_subscription<DetectedObjects>(
-    "input", rclcpp::QoS{1},
-    std::bind(&MultiObjectTracker::onMeasurement, this, std::placeholders::_1));
+  // detected_object_sub_ = create_subscription<DetectedObjects>(
+  //   "input", rclcpp::QoS{1},
+  //   std::bind(&MultiObjectTracker::onMeasurement, this, std::placeholders::_1));
   tracked_objects_pub_ = create_publisher<TrackedObjects>("output", rclcpp::QoS{1});
 
   // Get parameters
   double publish_rate = declare_parameter<double>("publish_rate");  // [hz]
   world_frame_id_ = declare_parameter<std::string>("world_frame_id");
   bool enable_delay_compensation{declare_parameter<bool>("enable_delay_compensation")};
+  declare_parameter("input_topic_names", std::vector<std::string>());
+  input_topic_names_ = get_parameter("input_topic_names").as_string_array();
+
+  // Set input topics
+  if (input_topic_names_.empty()) {
+    RCLCPP_ERROR(get_logger(), "Need a 'input_topic_names' parameter to be set before continuing");
+    return;
+  }
+  input_topic_size_ = input_topic_names_.size();
+  sub_objects_array_.resize(input_topic_size_);
+  objects_data_.resize(input_topic_size_);
+
+  for (size_t i = 0; i < input_topic_size_; i++) {
+    RCLCPP_DEBUG(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str());
+
+    std::function<void(const DetectedObjects::ConstSharedPtr msg)> func =
+      std::bind(&MultiObjectTracker::onData, this, std::placeholders::_1, i);
+    sub_objects_array_.at(i) =
+      create_subscription<DetectedObjects>(input_topic_names_.at(i), rclcpp::QoS{1}, func);
+  }
 
   // Create tf timer
   auto cti = std::make_shared<tf2_ros::CreateTimerROS>(
@@ -143,6 +163,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
+void MultiObjectTracker::onData(DetectedObjects::ConstSharedPtr msg, const size_t array_number)
+{
+  objects_data_.at(array_number) = msg;
+  // debug message
+  // std::cout << "Received data from topic " << input_topic_names_.at(array_number) << std::endl;
+
+  onMeasurement(msg);
+}
+
 void MultiObjectTracker::onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg)
 {
   // Get the time of the measurement

From 96be0ccba7ab71d38377e3c0ce4d8cf40ec8e4f7 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 11 Apr 2024 18:07:29 +0900
Subject: [PATCH 03/67] feat: check latest measurement time

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_core.hpp                 |  1 +
 .../src/multi_object_tracker_core.cpp             | 15 +++++++++++++--
 2 files changed, 14 insertions(+), 2 deletions(-)

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
index e621479d4e886..0554c2f1720da 100644
--- 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
@@ -86,6 +86,7 @@ class MultiObjectTracker : public rclcpp::Node
   // inputs
   rclcpp::Subscription<DetectedObjects>::SharedPtr sub_objects_{};
   std::vector<rclcpp::Subscription<DetectedObjects>::SharedPtr> sub_objects_array_{};
+  rclcpp::Time last_measurement_time_;
 
   std::vector<std::string> input_topic_names_{};
   size_t input_topic_size_{};
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index e55b0d0371c55..098b711d9b207 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -89,6 +89,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   input_topic_names_ = get_parameter("input_topic_names").as_string_array();
 
   // Set input topics
+  last_measurement_time_ = this->now();
   if (input_topic_names_.empty()) {
     RCLCPP_ERROR(get_logger(), "Need a 'input_topic_names' parameter to be set before continuing");
     return;
@@ -98,7 +99,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   objects_data_.resize(input_topic_size_);
 
   for (size_t i = 0; i < input_topic_size_; i++) {
-    RCLCPP_DEBUG(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str());
+    RCLCPP_INFO(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str());
 
     std::function<void(const DetectedObjects::ConstSharedPtr msg)> func =
       std::bind(&MultiObjectTracker::onData, this, std::placeholders::_1, i);
@@ -167,7 +168,17 @@ void MultiObjectTracker::onData(DetectedObjects::ConstSharedPtr msg, const size_
 {
   objects_data_.at(array_number) = msg;
   // debug message
-  // std::cout << "Received data from topic " << input_topic_names_.at(array_number) << std::endl;
+  RCLCPP_INFO(
+    get_logger(), "Received data from topic %s", input_topic_names_.at(array_number).c_str());
+  if (last_measurement_time_ > msg->header.stamp) {
+    const double delta_time = (last_measurement_time_ - msg->header.stamp).seconds() * 1e3;
+    RCLCPP_INFO(
+      get_logger(), "Received data is older than the last measurement time by %f ms", delta_time);
+    return;
+  }
+  if (last_measurement_time_ < msg->header.stamp) {
+    last_measurement_time_ = msg->header.stamp;
+  }
 
   onMeasurement(msg);
 }

From fd086dcd8bd323c9eaf42bb6201c8ad388a7c465 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 12 Apr 2024 15:32:41 +0900
Subject: [PATCH 04/67] feat: define input manager class

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker/CMakeLists.txt       |  1 +
 .../multi_object_tracker_core.hpp             |  2 +-
 .../processor/input_manager.hpp               | 77 +++++++++++++++++++
 .../processor/processor.hpp                   |  2 -
 .../src/multi_object_tracker_core.cpp         | 17 ++--
 .../src/processor/input_manager.cpp           | 76 ++++++++++++++++++
 .../src/processor/processor.cpp               |  2 -
 7 files changed, 164 insertions(+), 13 deletions(-)
 create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
 create mode 100644 perception/multi_object_tracker/src/processor/input_manager.cpp

diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt
index 46af1f9b9a1de..7d779d3bfc093 100644
--- a/perception/multi_object_tracker/CMakeLists.txt
+++ b/perception/multi_object_tracker/CMakeLists.txt
@@ -24,6 +24,7 @@ set(MULTI_OBJECT_TRACKER_SRC
   src/multi_object_tracker_core.cpp
   src/debugger.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/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp
index 0554c2f1720da..33450ccb54131 100644
--- 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
@@ -93,7 +93,7 @@ class MultiObjectTracker : public rclcpp::Node
   std::vector<DetectedObjects::ConstSharedPtr> objects_data_{};
 
   // callback functions
-  void onData(DetectedObjects::ConstSharedPtr msg, const size_t array_number);
+  void onData(const DetectedObjects::ConstSharedPtr msg, const size_t array_number);
   void onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg);
   void onTimer();
 
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
new file mode 100644
index 0000000000000..c8a9e22e4d616
--- /dev/null
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -0,0 +1,77 @@
+// 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__INPUT_MANAGER_HPP_
+#define MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
+
+#include <deque>
+#include <memory>
+#include <string>
+#include <vector>
+
+class InputManager
+{
+public:
+  explicit InputManager(rclcpp::Node & node);
+
+  void init(
+    const std::string & input_topic, const std::string & long_name, const std::string & short_name);
+
+  void setObjects(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
+
+private:
+  rclcpp::Node & node_;
+
+  std::string input_topic_;
+  std::string long_name_;
+  std::string short_name_;
+
+  rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr sub_objects_;
+
+  size_t que_size_{20};
+  std::deque<autoware_auto_perception_msgs::msg::DetectedObjects> objects_que_;
+
+  double expected_rate_;
+  double latency_mean_;
+  double latency_var_;
+
+  rclcpp::Time latest_measurement_time_;
+  rclcpp::Time latest_message_time_;
+};
+
+class InputManagers
+{
+public:
+  explicit InputManagers(rclcpp::Node & node);
+
+  void init(
+    const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names,
+    const std::vector<std::string> & short_names);
+
+  void getObjects(
+    const rclcpp::Time & now,
+    std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects);
+
+private:
+  rclcpp::Node & node_;
+
+  size_t input_size_;
+  std::vector<std::shared_ptr<InputManager>> input_managers_;
+};
+
+#endif  // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_
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
index 6d6e364d83a41..654c960463933 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp
@@ -11,8 +11,6 @@
 // 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_
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 098b711d9b207..669484b373367 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -75,12 +75,6 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   tf_listener_(tf_buffer_),
   last_published_time_(this->now())
 {
-  // Create publishers and subscribers
-  // detected_object_sub_ = create_subscription<DetectedObjects>(
-  //   "input", rclcpp::QoS{1},
-  //   std::bind(&MultiObjectTracker::onMeasurement, this, std::placeholders::_1));
-  tracked_objects_pub_ = create_publisher<TrackedObjects>("output", rclcpp::QoS{1});
-
   // Get parameters
   double publish_rate = declare_parameter<double>("publish_rate");  // [hz]
   world_frame_id_ = declare_parameter<std::string>("world_frame_id");
@@ -88,8 +82,13 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   declare_parameter("input_topic_names", std::vector<std::string>());
   input_topic_names_ = get_parameter("input_topic_names").as_string_array();
 
-  // Set input topics
+  // Initialize the last measurement time
   last_measurement_time_ = this->now();
+
+  // ROS interface - Publisher
+  tracked_objects_pub_ = create_publisher<TrackedObjects>("output", rclcpp::QoS{1});
+
+  // ROS interface - Subscribers
   if (input_topic_names_.empty()) {
     RCLCPP_ERROR(get_logger(), "Need a 'input_topic_names' parameter to be set before continuing");
     return;
@@ -164,9 +163,11 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
-void MultiObjectTracker::onData(DetectedObjects::ConstSharedPtr msg, const size_t array_number)
+void MultiObjectTracker::onData(
+  const DetectedObjects::ConstSharedPtr msg, const size_t array_number)
 {
   objects_data_.at(array_number) = msg;
+
   // debug message
   RCLCPP_INFO(
     get_logger(), "Received data from topic %s", input_topic_names_.at(array_number).c_str());
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
new file mode 100644
index 0000000000000..f7146c8bc9b29
--- /dev/null
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -0,0 +1,76 @@
+// 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.
+
+#include "multi_object_tracker/processor/input_manager.hpp"
+
+InputManager::InputManager(rclcpp::Node & node) : node_(node)
+{
+}
+
+void InputManager::init(
+  const std::string & input_topic, const std::string & long_name, const std::string & short_name)
+{
+  // Initialize parameters
+  input_topic_ = input_topic;
+  long_name_ = long_name;
+  short_name_ = short_name;
+
+  // Initialize subscription
+  std::function<void(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)>
+    func = std::bind(&InputManager::setObjects, this, std::placeholders::_1);
+  sub_objects_ = node_.create_subscription<autoware_auto_perception_msgs::msg::DetectedObjects>(
+    input_topic_, rclcpp::QoS{1}, func);
+
+  // Initialize latency statistics
+  expected_rate_ = 10.0;
+  latency_mean_ = 1 / expected_rate_;
+  latency_var_ = 0.0;
+}
+
+void InputManager::setObjects(
+  const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)
+{
+  const auto & object = *msg;
+  objects_que_.push_back(object);
+  if (objects_que_.size() > que_size_) {
+    objects_que_.pop_front();
+  }
+  latest_message_time_ = node_.now();
+  latest_measurement_time_ = object.header.stamp;
+
+  // Calculate latency
+  const double latency = (latest_message_time_ - latest_measurement_time_).seconds();
+
+  // Update latency statistics
+  constexpr double gain = 0.05;
+  latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency;
+  const double latency_delta = latency - latency_mean_;
+  latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta;
+}
+
+InputManagers::InputManagers(rclcpp::Node & node) : node_(node)
+{
+}
+
+void InputManagers::init(
+  const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names,
+  const std::vector<std::string> & short_names)
+{
+  input_size_ = input_topics.size();
+  for (size_t i = 0; i < input_size_; ++i) {
+    InputManager input_manager(node_);
+    input_manager.init(input_topics[i], long_names[i], short_names[i]);
+    input_managers_.push_back(std::make_shared<InputManager>(input_manager));
+  }
+}
diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp
index 0d56e16b431e9..8cfb64cab19be 100644
--- a/perception/multi_object_tracker/src/processor/processor.cpp
+++ b/perception/multi_object_tracker/src/processor/processor.cpp
@@ -11,8 +11,6 @@
 // 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.
-//
-//
 
 #include "multi_object_tracker/processor/processor.hpp"
 

From 1462eefe9c2b7eeb7a32e53a574a066e9f9ec229 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 12 Apr 2024 15:37:53 +0900
Subject: [PATCH 05/67] feat: interval measures

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../processor/input_manager.hpp               |  3 +++
 .../src/processor/input_manager.cpp           | 22 ++++++++++++++++---
 2 files changed, 22 insertions(+), 3 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index c8a9e22e4d616..9ed3074a8a3ee 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -46,9 +46,12 @@ class InputManager
   size_t que_size_{20};
   std::deque<autoware_auto_perception_msgs::msg::DetectedObjects> objects_que_;
 
+  bool is_time_initialized_{false};
   double expected_rate_;
   double latency_mean_;
   double latency_var_;
+  double interval_mean_;
+  double interval_var_;
 
   rclcpp::Time latest_measurement_time_;
   rclcpp::Time latest_message_time_;
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index f7146c8bc9b29..5ecd82731edcd 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -34,8 +34,10 @@ void InputManager::init(
 
   // Initialize latency statistics
   expected_rate_ = 10.0;
-  latency_mean_ = 1 / expected_rate_;
+  latency_mean_ = 0.10;  // [s]
   latency_var_ = 0.0;
+  interval_mean_ = 1 / expected_rate_;
+  interval_var_ = 0.0;
 }
 
 void InputManager::setObjects(
@@ -46,14 +48,28 @@ void InputManager::setObjects(
   if (objects_que_.size() > que_size_) {
     objects_que_.pop_front();
   }
-  latest_message_time_ = node_.now();
+
+  // Filter parameters
+  constexpr double gain = 0.05;
+  const auto now = node_.now();
+
+  // Calculate interval, Update interval statistics
+  if (is_time_initialized_) {
+    const double interval = (now - latest_message_time_).seconds();
+    interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval;
+    const double interval_delta = interval - interval_mean_;
+    interval_var_ = (1.0 - gain) * interval_var_ + gain * interval_delta * interval_delta;
+  }
+
+  // Update time
+  latest_message_time_ = now;
   latest_measurement_time_ = object.header.stamp;
+  if (!is_time_initialized_) is_time_initialized_ = true;
 
   // Calculate latency
   const double latency = (latest_message_time_ - latest_measurement_time_).seconds();
 
   // Update latency statistics
-  constexpr double gain = 0.05;
   latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency;
   const double latency_delta = latency - latency_mean_;
   latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta;

From 7757212d5dd871026b9d75f1bd7d547df58b65c1 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 12 Apr 2024 17:18:22 +0900
Subject: [PATCH 06/67] feat: store and sort inputs PoC

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_node.param.yaml      |   2 +
 .../multi_object_tracker_core.hpp             |   4 +-
 .../src/multi_object_tracker_core.cpp         | 100 ++++++++++++------
 3 files changed, 70 insertions(+), 36 deletions(-)

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
index 09b9adee8caa8..494c5f2cd1e28 100644
--- a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml
+++ b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml
@@ -21,3 +21,5 @@
     diagnostics_error_delay: 1.0     # [sec]
 
     input_topic_names: ["/perception/object_recognition/detection/objects"]
+    input_names_long: [merged_objects]
+    input_names_short: [all]
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
index 33450ccb54131..75c594e01171b 100644
--- 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
@@ -48,6 +48,7 @@
 #include <memory>
 #include <string>
 #include <unordered_map>
+#include <utility>
 #include <vector>
 
 namespace multi_object_tracker
@@ -90,7 +91,7 @@ class MultiObjectTracker : public rclcpp::Node
 
   std::vector<std::string> input_topic_names_{};
   size_t input_topic_size_{};
-  std::vector<DetectedObjects::ConstSharedPtr> objects_data_{};
+  std::vector<std::pair<rclcpp::Time, DetectedObjects>> objects_data_{};
 
   // callback functions
   void onData(const DetectedObjects::ConstSharedPtr msg, const size_t array_number);
@@ -98,6 +99,7 @@ class MultiObjectTracker : public rclcpp::Node
   void onTimer();
 
   // publish processes
+  void runProcess(const DetectedObjects::ConstSharedPtr input_objects_msg);
   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/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 669484b373367..e9f80c902b664 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -95,7 +95,6 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   }
   input_topic_size_ = input_topic_names_.size();
   sub_objects_array_.resize(input_topic_size_);
-  objects_data_.resize(input_topic_size_);
 
   for (size_t i = 0; i < input_topic_size_; i++) {
     RCLCPP_INFO(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str());
@@ -166,7 +165,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
 void MultiObjectTracker::onData(
   const DetectedObjects::ConstSharedPtr msg, const size_t array_number)
 {
-  objects_data_.at(array_number) = msg;
+  std::pair<rclcpp::Time, DetectedObjects> pair = std::make_pair(msg->header.stamp, *msg);
+  objects_data_.push_back(pair);
 
   // debug message
   RCLCPP_INFO(
@@ -181,7 +181,7 @@ void MultiObjectTracker::onData(
     last_measurement_time_ = msg->header.stamp;
   }
 
-  onMeasurement(msg);
+  // onMeasurement(msg);
 }
 
 void MultiObjectTracker::onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg)
@@ -190,24 +190,79 @@ void MultiObjectTracker::onMeasurement(const DetectedObjects::ConstSharedPtr inp
   const rclcpp::Time measurement_time =
     rclcpp::Time(input_objects_msg->header.stamp, this->now().get_clock_type());
 
-  /* keep the latest input stamp and check transform*/
   debugger_->startMeasurementTime(this->now(), measurement_time);
+  runProcess(input_objects_msg);
+  debugger_->endMeasurementTime(this->now());
+
+  // Publish objects if the timer is not enabled
+  if (publish_timer_ == nullptr) {
+    // Publish if the delay compensation is disabled
+    publish(measurement_time);
+  } else {
+    // Publish if the next publish time is close
+    const double minimum_publish_interval = publisher_period_ * 0.70;  // 70% of the period
+    if ((this->now() - last_published_time_).seconds() > minimum_publish_interval) {
+      checkAndPublish(this->now());
+    }
+  }
+}
+
+void MultiObjectTracker::onTimer()
+{
+  const rclcpp::Time current_time = this->now();
+
+  // Check the publish period
+  const auto elapsed_time = (current_time - last_published_time_).seconds();
+  // If the elapsed time is over the period, publish objects with prediction
+  constexpr double maximum_latency_ratio = 1.11;  // 11% margin
+  const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio;
+
+  if (elapsed_time > maximum_publish_latency) {
+    // update objects
+    if (objects_data_.empty()) {
+      return;
+    }
+    // updated from accumulated objects
+    // sort by time
+    sort(objects_data_.begin(), objects_data_.end(), [](const auto & a, const auto & b) {
+      return a.first.seconds() < b.first.seconds();
+    });
+
+    // run onMeasurement for each queue
+    while (!objects_data_.empty()) {
+      const auto & pair = objects_data_.front();
+      runProcess(std::make_shared<DetectedObjects>(pair.second));
+      objects_data_.erase(objects_data_.begin());
+    }
+
+    // Publish
+    checkAndPublish(current_time);
+  }
+}
+
+void MultiObjectTracker::runProcess(const DetectedObjects::ConstSharedPtr input_objects_msg)
+{
+  // Get the time of the measurement
+  const rclcpp::Time measurement_time =
+    rclcpp::Time(input_objects_msg->header.stamp, this->now().get_clock_type());
+
+  // Get the transform of the self frame
   const auto self_transform =
     getTransformAnonymous(tf_buffer_, "base_link", world_frame_id_, measurement_time);
   if (!self_transform) {
     return;
   }
-  /* transform to world coordinate */
+
+  // Transform the objects to the world frame
   DetectedObjects transformed_objects;
   if (!object_recognition_utils::transformObjects(
         *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) {
     return;
   }
 
-  ////// Tracker Process
-  //// Associate and update
   /* prediction */
   processor_->predict(measurement_time);
+
   /* object association */
   std::unordered_map<int, int> direct_assignment, reverse_assignment;
   {
@@ -218,40 +273,15 @@ void MultiObjectTracker::onMeasurement(const DetectedObjects::ConstSharedPtr inp
       detected_objects, list_tracker);  // row : tracker, col : measurement
     data_association_->assign(score_matrix, direct_assignment, reverse_assignment);
   }
+
   /* tracker update */
   processor_->update(transformed_objects, *self_transform, direct_assignment);
+
   /* tracker pruning */
   processor_->prune(measurement_time);
+
   /* spawn new tracker */
   processor_->spawn(transformed_objects, *self_transform, reverse_assignment);
-
-  // debugger time
-  debugger_->endMeasurementTime(this->now());
-
-  // Publish objects if the timer is not enabled
-  if (publish_timer_ == nullptr) {
-    // Publish if the delay compensation is disabled
-    publish(measurement_time);
-  } else {
-    // Publish if the next publish time is close
-    const double minimum_publish_interval = publisher_period_ * 0.70;  // 70% of the period
-    if ((this->now() - last_published_time_).seconds() > minimum_publish_interval) {
-      checkAndPublish(this->now());
-    }
-  }
-}
-
-void MultiObjectTracker::onTimer()
-{
-  const rclcpp::Time current_time = this->now();
-  // Check the publish period
-  const auto elapsed_time = (current_time - last_published_time_).seconds();
-  // If the elapsed time is over the period, publish objects with prediction
-  constexpr double maximum_latency_ratio = 1.11;  // 11% margin
-  const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio;
-  if (elapsed_time > maximum_publish_latency) {
-    checkAndPublish(current_time);
-  }
 }
 
 void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time)

From 2a443e70c5f50a23fe193fab8084aa571735ef02 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 12 Apr 2024 17:33:07 +0900
Subject: [PATCH 07/67] chore: rename classes

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../processor/input_manager.hpp                | 11 +++++------
 .../src/processor/input_manager.cpp            | 18 +++++++++---------
 2 files changed, 14 insertions(+), 15 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 9ed3074a8a3ee..b441ebac57def 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -24,10 +24,10 @@
 #include <string>
 #include <vector>
 
-class InputManager
+class InputStream
 {
 public:
-  explicit InputManager(rclcpp::Node & node);
+  explicit InputStream(rclcpp::Node & node);
 
   void init(
     const std::string & input_topic, const std::string & long_name, const std::string & short_name);
@@ -57,10 +57,9 @@ class InputManager
   rclcpp::Time latest_message_time_;
 };
 
-class InputManagers
-{
+class InputManager{
 public:
-  explicit InputManagers(rclcpp::Node & node);
+  explicit InputManager(rclcpp::Node & node);
 
   void init(
     const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names,
@@ -74,7 +73,7 @@ class InputManagers
   rclcpp::Node & node_;
 
   size_t input_size_;
-  std::vector<std::shared_ptr<InputManager>> input_managers_;
+  std::vector<std::shared_ptr<InputStream>> input_streams_;
 };
 
 #endif  // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 5ecd82731edcd..9e767bcc63cb6 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -14,11 +14,11 @@
 
 #include "multi_object_tracker/processor/input_manager.hpp"
 
-InputManager::InputManager(rclcpp::Node & node) : node_(node)
+InputStream::InputStream(rclcpp::Node & node) : node_(node)
 {
 }
 
-void InputManager::init(
+void InputStream::init(
   const std::string & input_topic, const std::string & long_name, const std::string & short_name)
 {
   // Initialize parameters
@@ -28,7 +28,7 @@ void InputManager::init(
 
   // Initialize subscription
   std::function<void(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)>
-    func = std::bind(&InputManager::setObjects, this, std::placeholders::_1);
+    func = std::bind(&InputStream::setObjects, this, std::placeholders::_1);
   sub_objects_ = node_.create_subscription<autoware_auto_perception_msgs::msg::DetectedObjects>(
     input_topic_, rclcpp::QoS{1}, func);
 
@@ -40,7 +40,7 @@ void InputManager::init(
   interval_var_ = 0.0;
 }
 
-void InputManager::setObjects(
+void InputStream::setObjects(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)
 {
   const auto & object = *msg;
@@ -75,18 +75,18 @@ void InputManager::setObjects(
   latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta;
 }
 
-InputManagers::InputManagers(rclcpp::Node & node) : node_(node)
+InputManager::InputManager(rclcpp::Node & node) : node_(node)
 {
 }
 
-void InputManagers::init(
+void InputManager::init(
   const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names,
   const std::vector<std::string> & short_names)
 {
   input_size_ = input_topics.size();
   for (size_t i = 0; i < input_size_; ++i) {
-    InputManager input_manager(node_);
-    input_manager.init(input_topics[i], long_names[i], short_names[i]);
-    input_managers_.push_back(std::make_shared<InputManager>(input_manager));
+    InputStream input_stream(node_);
+    input_stream.init(input_topics[i], long_names[i], short_names[i]);
+    input_streams_.push_back(std::make_shared<InputStream>(input_stream));
   }
 }

From 866b656c67ed1b589c28f61b8f8ab2b464ae32d0 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 12 Apr 2024 18:22:23 +0900
Subject: [PATCH 08/67] feat: object collector

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../processor/input_manager.hpp               |  6 ++-
 .../src/multi_object_tracker_core.cpp         | 36 +++++++-------
 .../src/processor/input_manager.cpp           | 48 +++++++++++++++++++
 3 files changed, 70 insertions(+), 20 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index b441ebac57def..55a264f92c16b 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -33,6 +33,9 @@ class InputStream
     const std::string & input_topic, const std::string & long_name, const std::string & short_name);
 
   void setObjects(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
+  void getObjectsOlderThan(
+    const rclcpp::Time & time, const double duration,
+    std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects);
 
 private:
   rclcpp::Node & node_;
@@ -57,7 +60,8 @@ class InputStream
   rclcpp::Time latest_message_time_;
 };
 
-class InputManager{
+class InputManager
+{
 public:
   explicit InputManager(rclcpp::Node & node);
 
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index e9f80c902b664..1c3b2aa98129c 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -217,27 +217,25 @@ void MultiObjectTracker::onTimer()
   constexpr double maximum_latency_ratio = 1.11;  // 11% margin
   const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio;
 
-  if (elapsed_time > maximum_publish_latency) {
-    // update objects
-    if (objects_data_.empty()) {
-      return;
-    }
-    // updated from accumulated objects
-    // sort by time
-    sort(objects_data_.begin(), objects_data_.end(), [](const auto & a, const auto & b) {
-      return a.first.seconds() < b.first.seconds();
-    });
-
-    // run onMeasurement for each queue
-    while (!objects_data_.empty()) {
-      const auto & pair = objects_data_.front();
-      runProcess(std::make_shared<DetectedObjects>(pair.second));
-      objects_data_.erase(objects_data_.begin());
-    }
+  if (elapsed_time < maximum_publish_latency) return;
+
+  // update trackers from accumulated input objects
+  if (objects_data_.empty()) return;
 
-    // Publish
-    checkAndPublish(current_time);
+  // sort by time
+  sort(objects_data_.begin(), objects_data_.end(), [](const auto & a, const auto & b) {
+    return a.first.seconds() < b.first.seconds();
+  });
+
+  // run onMeasurement for each queue
+  while (!objects_data_.empty()) {
+    const auto & pair = objects_data_.front();
+    runProcess(std::make_shared<DetectedObjects>(pair.second));
+    objects_data_.erase(objects_data_.begin());
   }
+
+  // Publish
+  checkAndPublish(current_time);
 }
 
 void MultiObjectTracker::runProcess(const DetectedObjects::ConstSharedPtr input_objects_msg)
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 9e767bcc63cb6..ead42da56487f 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -75,6 +75,25 @@ void InputStream::setObjects(
   latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta;
 }
 
+void InputStream::getObjectsOlderThan(
+  const rclcpp::Time & time, const double duration,
+  std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
+{
+  objects.clear();
+  for (const auto & object : objects_que_) {
+    // Skip if the object is older than the specified duration
+    const rclcpp::Time object_time = rclcpp::Time(object.header.stamp);
+    const double time_diff = (object_time - time).seconds();
+    if (time_diff > duration) {
+      continue;
+    }
+    // Add the object if the object is older than the specified time
+    if (time_diff >= 0.0) {
+      objects.push_back(object);
+    }
+  }
+}
+
 InputManager::InputManager(rclcpp::Node & node) : node_(node)
 {
 }
@@ -90,3 +109,32 @@ void InputManager::init(
     input_streams_.push_back(std::make_shared<InputStream>(input_stream));
   }
 }
+
+void InputManager::getObjects(
+  const rclcpp::Time & now,
+  std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
+{
+  objects.clear();
+
+  // Get proper latency
+  constexpr double target_latency = 0.15;   // [s], measurement to tracking latency, as much as the
+                                            // detector latency, the less total latency
+  constexpr double acceptable_band = 0.15;  // [s], acceptable band from the target latency
+  rclcpp::Time time = now - rclcpp::Duration::from_seconds(target_latency);
+
+  // Get objects from all input streams
+  for (const auto & input_stream : input_streams_) {
+    std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_tmp;
+    input_stream->getObjectsOlderThan(time, acceptable_band, objects_tmp);
+    objects.insert(objects.end(), objects_tmp.begin(), objects_tmp.end());
+  }
+
+  // Sort objects by timestamp
+  std::sort(
+    objects.begin(), objects.end(),
+    [](
+      const autoware_auto_perception_msgs::msg::DetectedObjects & a,
+      const autoware_auto_perception_msgs::msg::DetectedObjects & b) {
+      return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0;
+    });
+}

From 4b71b57a7430ecd4fd854ee76b0a714938fc4738 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 12 Apr 2024 19:33:38 +0900
Subject: [PATCH 09/67] impl input manager, no subscribe

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_node.param.yaml      |  4 +-
 .../multi_object_tracker_core.hpp             |  6 +-
 .../processor/input_manager.hpp               |  2 +-
 .../src/multi_object_tracker_core.cpp         | 73 ++++++++++++-------
 .../src/processor/input_manager.cpp           | 14 ++--
 5 files changed, 63 insertions(+), 36 deletions(-)

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
index 494c5f2cd1e28..945cf44113a06 100644
--- a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml
+++ b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml
@@ -21,5 +21,5 @@
     diagnostics_error_delay: 1.0     # [sec]
 
     input_topic_names: ["/perception/object_recognition/detection/objects"]
-    input_names_long: [merged_objects]
-    input_names_short: [all]
+    input_names_long: ["merged_objects"]
+    input_names_short: ["all"]
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
index 75c594e01171b..e3a2864b1faaf 100644
--- 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/input_manager.hpp"
 #include "multi_object_tracker/processor/processor.hpp"
 #include "multi_object_tracker/tracker/model/tracker_base.hpp"
 
@@ -89,6 +90,9 @@ class MultiObjectTracker : public rclcpp::Node
   std::vector<rclcpp::Subscription<DetectedObjects>::SharedPtr> sub_objects_array_{};
   rclcpp::Time last_measurement_time_;
 
+  // input manager
+  std::unique_ptr<InputManager> input_manager_;
+
   std::vector<std::string> input_topic_names_{};
   size_t input_topic_size_{};
   std::vector<std::pair<rclcpp::Time, DetectedObjects>> objects_data_{};
@@ -99,7 +103,7 @@ class MultiObjectTracker : public rclcpp::Node
   void onTimer();
 
   // publish processes
-  void runProcess(const DetectedObjects::ConstSharedPtr input_objects_msg);
+  void runProcess(const DetectedObjects & input_objects_msg);
   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/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 55a264f92c16b..62ddee4a9ec6a 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -69,7 +69,7 @@ class InputManager
     const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names,
     const std::vector<std::string> & short_names);
 
-  void getObjects(
+  bool getObjects(
     const rclcpp::Time & now,
     std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects);
 
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 1c3b2aa98129c..f0c9a874c7a14 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -79,8 +79,13 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   double publish_rate = declare_parameter<double>("publish_rate");  // [hz]
   world_frame_id_ = declare_parameter<std::string>("world_frame_id");
   bool enable_delay_compensation{declare_parameter<bool>("enable_delay_compensation")};
+
   declare_parameter("input_topic_names", std::vector<std::string>());
-  input_topic_names_ = get_parameter("input_topic_names").as_string_array();
+  declare_parameter("input_names_long", std::vector<std::string>());
+  declare_parameter("input_names_short", std::vector<std::string>());
+  std::vector<std::string> input_topic_names = get_parameter("input_topic_names").as_string_array();
+  std::vector<std::string> input_names_long = get_parameter("input_names_long").as_string_array();
+  std::vector<std::string> input_names_short = get_parameter("input_names_short").as_string_array();
 
   // Initialize the last measurement time
   last_measurement_time_ = this->now();
@@ -89,21 +94,29 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   tracked_objects_pub_ = create_publisher<TrackedObjects>("output", rclcpp::QoS{1});
 
   // ROS interface - Subscribers
-  if (input_topic_names_.empty()) {
+  if (input_topic_names.empty()) {
     RCLCPP_ERROR(get_logger(), "Need a 'input_topic_names' parameter to be set before continuing");
     return;
   }
-  input_topic_size_ = input_topic_names_.size();
-  sub_objects_array_.resize(input_topic_size_);
+  input_topic_size_ = input_topic_names.size();
 
+  // print input information
   for (size_t i = 0; i < input_topic_size_; i++) {
-    RCLCPP_INFO(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str());
-
-    std::function<void(const DetectedObjects::ConstSharedPtr msg)> func =
-      std::bind(&MultiObjectTracker::onData, this, std::placeholders::_1, i);
-    sub_objects_array_.at(i) =
-      create_subscription<DetectedObjects>(input_topic_names_.at(i), rclcpp::QoS{1}, func);
+    RCLCPP_INFO(get_logger(), "Subscribing to %s %s %s", input_topic_names.at(i).c_str(), input_names_long.at(i).c_str(), input_names_short.at(i).c_str());
   }
+  
+  input_manager_ = std::make_unique<InputManager>(*this);
+  input_manager_->init(input_topic_names_, input_names_long, input_names_short);
+
+  // sub_objects_array_.resize(input_topic_size_);
+
+  // for (size_t i = 0; i < input_topic_size_; i++) {
+  // RCLCPP_INFO(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str());
+  // std::function<void(const DetectedObjects::ConstSharedPtr msg)> func =
+  //   std::bind(&MultiObjectTracker::onData, this, std::placeholders::_1, i);
+  // sub_objects_array_.at(i) =
+  //   create_subscription<DetectedObjects>(input_topic_names_.at(i), rclcpp::QoS{1}, func);
+  // }
 
   // Create tf timer
   auto cti = std::make_shared<tf2_ros::CreateTimerROS>(
@@ -191,7 +204,7 @@ void MultiObjectTracker::onMeasurement(const DetectedObjects::ConstSharedPtr inp
     rclcpp::Time(input_objects_msg->header.stamp, this->now().get_clock_type());
 
   debugger_->startMeasurementTime(this->now(), measurement_time);
-  runProcess(input_objects_msg);
+  runProcess(*input_objects_msg);
   debugger_->endMeasurementTime(this->now());
 
   // Publish objects if the timer is not enabled
@@ -219,30 +232,36 @@ void MultiObjectTracker::onTimer()
 
   if (elapsed_time < maximum_publish_latency) return;
 
-  // update trackers from accumulated input objects
-  if (objects_data_.empty()) return;
-
-  // sort by time
-  sort(objects_data_.begin(), objects_data_.end(), [](const auto & a, const auto & b) {
-    return a.first.seconds() < b.first.seconds();
-  });
-
-  // run onMeasurement for each queue
-  while (!objects_data_.empty()) {
-    const auto & pair = objects_data_.front();
-    runProcess(std::make_shared<DetectedObjects>(pair.second));
-    objects_data_.erase(objects_data_.begin());
+  // // update trackers from accumulated input objects
+  // if (objects_data_.empty()) return;
+
+  // // sort by time
+  // sort(objects_data_.begin(), objects_data_.end(), [](const auto & a, const auto & b) {
+  //   return a.first.seconds() < b.first.seconds();
+  // });
+  std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_data;
+  if (input_manager_->getObjects(current_time, objects_data)) {
+    for (const auto & objects : objects_data) {
+      runProcess(objects);
+    }
   }
 
+  // // run onMeasurement for each queue
+  // while (!objects_data_.empty()) {
+  //   const auto & pair = objects_data_.front();
+  //   runProcess(std::make_shared<DetectedObjects>(pair.second));
+  //   objects_data_.erase(objects_data_.begin());
+  // }
+
   // Publish
   checkAndPublish(current_time);
 }
 
-void MultiObjectTracker::runProcess(const DetectedObjects::ConstSharedPtr input_objects_msg)
+void MultiObjectTracker::runProcess(const DetectedObjects & input_objects_msg)
 {
   // Get the time of the measurement
   const rclcpp::Time measurement_time =
-    rclcpp::Time(input_objects_msg->header.stamp, this->now().get_clock_type());
+    rclcpp::Time(input_objects_msg.header.stamp, this->now().get_clock_type());
 
   // Get the transform of the self frame
   const auto self_transform =
@@ -254,7 +273,7 @@ void MultiObjectTracker::runProcess(const DetectedObjects::ConstSharedPtr input_
   // Transform the objects to the world frame
   DetectedObjects transformed_objects;
   if (!object_recognition_utils::transformObjects(
-        *input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) {
+        input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) {
     return;
   }
 
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index ead42da56487f..be7b91f848a04 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "multi_object_tracker/processor/input_manager.hpp"
+#include <cassert>
 
 InputStream::InputStream(rclcpp::Node & node) : node_(node)
 {
@@ -99,18 +100,19 @@ InputManager::InputManager(rclcpp::Node & node) : node_(node)
 }
 
 void InputManager::init(
-  const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names,
-  const std::vector<std::string> & short_names)
+  const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names, const std::vector<std::string> & short_names)
 {
   input_size_ = input_topics.size();
-  for (size_t i = 0; i < input_size_; ++i) {
+  
+  for (size_t i = 0; i < input_size_; i++) {
+
     InputStream input_stream(node_);
-    input_stream.init(input_topics[i], long_names[i], short_names[i]);
+    input_stream.init(input_topics.at(i), long_names.at(i), short_names.at(i));
     input_streams_.push_back(std::make_shared<InputStream>(input_stream));
   }
 }
 
-void InputManager::getObjects(
+bool InputManager::getObjects(
   const rclcpp::Time & now,
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
 {
@@ -137,4 +139,6 @@ void InputManager::getObjects(
       const autoware_auto_perception_msgs::msg::DetectedObjects & b) {
       return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0;
     });
+
+  return !objects.empty();
 }

From a9fa209b16ca9793f5ecbee6170e4441bec38f7d Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 15 Apr 2024 10:14:19 +0900
Subject: [PATCH 10/67] fix: subscribe and trigger callback

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../src/multi_object_tracker_core.cpp         |  2 +-
 .../src/processor/input_manager.cpp           | 20 ++++++++++++++++---
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index f0c9a874c7a14..1b5201d6ae7d3 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -106,7 +106,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   }
   
   input_manager_ = std::make_unique<InputManager>(*this);
-  input_manager_->init(input_topic_names_, input_names_long, input_names_short);
+  input_manager_->init(input_topic_names, input_names_long, input_names_short);
 
   // sub_objects_array_.resize(input_topic_size_);
 
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index be7b91f848a04..327bb2df7a35a 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -27,12 +27,20 @@ void InputStream::init(
   long_name_ = long_name;
   short_name_ = short_name;
 
+  // debug message
+  RCLCPP_INFO(
+    node_.get_logger(), "Initializing %s input stream from %s", long_name_.c_str(),
+    input_topic_.c_str());
+
   // Initialize subscription
   std::function<void(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)>
     func = std::bind(&InputStream::setObjects, this, std::placeholders::_1);
   sub_objects_ = node_.create_subscription<autoware_auto_perception_msgs::msg::DetectedObjects>(
     input_topic_, rclcpp::QoS{1}, func);
 
+  // Initialize queue
+  objects_que_.clear();
+
   // Initialize latency statistics
   expected_rate_ = 10.0;
   latency_mean_ = 0.10;  // [s]
@@ -44,7 +52,11 @@ void InputStream::init(
 void InputStream::setObjects(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)
 {
-  const auto & object = *msg;
+  // debug message
+  RCLCPP_INFO(
+    node_.get_logger(), "Received %s message from %s", long_name_.c_str(), input_topic_.c_str());
+
+  const autoware_auto_perception_msgs::msg::DetectedObjects object = *msg;
   objects_que_.push_back(object);
   if (objects_que_.size() > que_size_) {
     objects_que_.pop_front();
@@ -103,9 +115,11 @@ void InputManager::init(
   const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names, const std::vector<std::string> & short_names)
 {
   input_size_ = input_topics.size();
-  
-  for (size_t i = 0; i < input_size_; i++) {
+  RCLCPP_INFO(node_.get_logger(), "Initializing input manager with %zu input streams", input_size_);
+  assert(input_size_ == long_names.size());
+  assert(input_size_ == short_names.size());
 
+  for (size_t i = 0; i < input_size_; i++) {
     InputStream input_stream(node_);
     input_stream.init(input_topics.at(i), long_names.at(i), short_names.at(i));
     input_streams_.push_back(std::make_shared<InputStream>(input_stream));

From cf2874d56aee6d98a30f7e87297b0e62c84e8b5b Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 15 Apr 2024 12:06:24 +0900
Subject: [PATCH 11/67] fix: subscriber and callbacks are working

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../processor/input_manager.hpp               |  7 ++-
 .../src/processor/input_manager.cpp           | 51 ++++++++++++++-----
 2 files changed, 42 insertions(+), 16 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 62ddee4a9ec6a..08bdd44269559 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -33,6 +33,7 @@ class InputStream
     const std::string & input_topic, const std::string & long_name, const std::string & short_name);
 
   void setObjects(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
+
   void getObjectsOlderThan(
     const rclcpp::Time & time, const double duration,
     std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects);
@@ -44,8 +45,6 @@ class InputStream
   std::string long_name_;
   std::string short_name_;
 
-  rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr sub_objects_;
-
   size_t que_size_{20};
   std::deque<autoware_auto_perception_msgs::msg::DetectedObjects> objects_que_;
 
@@ -75,6 +74,10 @@ class InputManager
 
 private:
   rclcpp::Node & node_;
+  std::vector<rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr>
+    sub_objects_array_{};
+
+  bool is_initialized_{false};
 
   size_t input_size_;
   std::vector<std::shared_ptr<InputStream>> input_streams_;
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 327bb2df7a35a..55d744ef47511 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -13,7 +13,9 @@
 // limitations under the License.
 
 #include "multi_object_tracker/processor/input_manager.hpp"
+
 #include <cassert>
+#include <functional>
 
 InputStream::InputStream(rclcpp::Node & node) : node_(node)
 {
@@ -27,17 +29,6 @@ void InputStream::init(
   long_name_ = long_name;
   short_name_ = short_name;
 
-  // debug message
-  RCLCPP_INFO(
-    node_.get_logger(), "Initializing %s input stream from %s", long_name_.c_str(),
-    input_topic_.c_str());
-
-  // Initialize subscription
-  std::function<void(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)>
-    func = std::bind(&InputStream::setObjects, this, std::placeholders::_1);
-  sub_objects_ = node_.create_subscription<autoware_auto_perception_msgs::msg::DetectedObjects>(
-    input_topic_, rclcpp::QoS{1}, func);
-
   // Initialize queue
   objects_que_.clear();
 
@@ -54,7 +45,8 @@ void InputStream::setObjects(
 {
   // debug message
   RCLCPP_INFO(
-    node_.get_logger(), "Received %s message from %s", long_name_.c_str(), input_topic_.c_str());
+    node_.get_logger(), "InputStream::setObjects Received %s message from %s at %d",
+    long_name_.c_str(), input_topic_.c_str(), msg->header.stamp.sec);
 
   const autoware_auto_perception_msgs::msg::DetectedObjects object = *msg;
   objects_que_.push_back(object);
@@ -62,6 +54,9 @@ void InputStream::setObjects(
     objects_que_.pop_front();
   }
 
+  // RCLCPP_INFO(
+  //   node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size());
+
   // Filter parameters
   constexpr double gain = 0.05;
   const auto now = node_.now();
@@ -112,17 +107,39 @@ InputManager::InputManager(rclcpp::Node & node) : node_(node)
 }
 
 void InputManager::init(
-  const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names, const std::vector<std::string> & short_names)
+  const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names,
+  const std::vector<std::string> & short_names)
 {
   input_size_ = input_topics.size();
-  RCLCPP_INFO(node_.get_logger(), "Initializing input manager with %zu input streams", input_size_);
+
+  // Check input sizes
+  RCLCPP_INFO(
+    node_.get_logger(), "InputManager::init Initializing input manager with %zu input streams",
+    input_size_);
   assert(input_size_ == long_names.size());
   assert(input_size_ == short_names.size());
 
+  sub_objects_array_.resize(input_size_);
+
   for (size_t i = 0; i < input_size_; i++) {
     InputStream input_stream(node_);
     input_stream.init(input_topics.at(i), long_names.at(i), short_names.at(i));
     input_streams_.push_back(std::make_shared<InputStream>(input_stream));
+
+    // Set subscription
+    RCLCPP_INFO(
+      node_.get_logger(), "InputManager::init Initializing %s input stream from %s",
+      long_names.at(i).c_str(), input_topics.at(i).c_str());
+    std::function<void(
+      const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)>
+      func = std::bind(&InputStream::setObjects, input_stream, std::placeholders::_1);
+    sub_objects_array_.at(i) =
+      node_.create_subscription<autoware_auto_perception_msgs::msg::DetectedObjects>(
+        input_topics.at(i), rclcpp::QoS{1}, func);
+  }
+
+  if (input_size_ > 0) {
+    is_initialized_ = true;
   }
 }
 
@@ -130,6 +147,8 @@ bool InputManager::getObjects(
   const rclcpp::Time & now,
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
 {
+  RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Getting objects from input streams");
+
   objects.clear();
 
   // Get proper latency
@@ -154,5 +173,9 @@ bool InputManager::getObjects(
       return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0;
     });
 
+  RCLCPP_INFO(
+    node_.get_logger(), "InputManager::getObjects Got %zu objects from input streams",
+    objects.size());
+
   return !objects.empty();
 }

From 246cb4a4d1df6ce40b6cd2f82fe5752ede63da8f Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 15 Apr 2024 13:10:14 +0900
Subject: [PATCH 12/67] fix: callback object is fixed, tracker is working

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../processor/input_manager.hpp               |  2 +-
 .../src/multi_object_tracker_core.cpp         | 32 +++-------------
 .../src/processor/input_manager.cpp           | 38 +++++++++++++++----
 3 files changed, 38 insertions(+), 34 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 08bdd44269559..4afcf2a817675 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -45,7 +45,7 @@ class InputStream
   std::string long_name_;
   std::string short_name_;
 
-  size_t que_size_{20};
+  size_t que_size_{50};
   std::deque<autoware_auto_perception_msgs::msg::DetectedObjects> objects_que_;
 
   bool is_time_initialized_{false};
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 1b5201d6ae7d3..371a6d0996067 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -102,22 +102,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
 
   // print input information
   for (size_t i = 0; i < input_topic_size_; i++) {
-    RCLCPP_INFO(get_logger(), "Subscribing to %s %s %s", input_topic_names.at(i).c_str(), input_names_long.at(i).c_str(), input_names_short.at(i).c_str());
+    RCLCPP_INFO(
+      get_logger(), "Subscribing to %s %s %s", input_topic_names.at(i).c_str(),
+      input_names_long.at(i).c_str(), input_names_short.at(i).c_str());
   }
-  
+
+  // Initialize input manager
   input_manager_ = std::make_unique<InputManager>(*this);
   input_manager_->init(input_topic_names, input_names_long, input_names_short);
 
-  // sub_objects_array_.resize(input_topic_size_);
-
-  // for (size_t i = 0; i < input_topic_size_; i++) {
-  // RCLCPP_INFO(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str());
-  // std::function<void(const DetectedObjects::ConstSharedPtr msg)> func =
-  //   std::bind(&MultiObjectTracker::onData, this, std::placeholders::_1, i);
-  // sub_objects_array_.at(i) =
-  //   create_subscription<DetectedObjects>(input_topic_names_.at(i), rclcpp::QoS{1}, func);
-  // }
-
   // Create tf timer
   auto cti = std::make_shared<tf2_ros::CreateTimerROS>(
     this->get_node_base_interface(), this->get_node_timers_interface());
@@ -232,13 +225,7 @@ void MultiObjectTracker::onTimer()
 
   if (elapsed_time < maximum_publish_latency) return;
 
-  // // update trackers from accumulated input objects
-  // if (objects_data_.empty()) return;
-
-  // // sort by time
-  // sort(objects_data_.begin(), objects_data_.end(), [](const auto & a, const auto & b) {
-  //   return a.first.seconds() < b.first.seconds();
-  // });
+  // get objects from the input manager and run process
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_data;
   if (input_manager_->getObjects(current_time, objects_data)) {
     for (const auto & objects : objects_data) {
@@ -246,13 +233,6 @@ void MultiObjectTracker::onTimer()
     }
   }
 
-  // // run onMeasurement for each queue
-  // while (!objects_data_.empty()) {
-  //   const auto & pair = objects_data_.front();
-  //   runProcess(std::make_shared<DetectedObjects>(pair.second));
-  //   objects_data_.erase(objects_data_.begin());
-  // }
-
   // Publish
   checkAndPublish(current_time);
 }
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 55d744ef47511..928b3874b4b8a 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -54,8 +54,7 @@ void InputStream::setObjects(
     objects_que_.pop_front();
   }
 
-  // RCLCPP_INFO(
-  //   node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size());
+  RCLCPP_INFO(node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size());
 
   // Filter parameters
   constexpr double gain = 0.05;
@@ -88,18 +87,43 @@ void InputStream::getObjectsOlderThan(
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
 {
   objects.clear();
+
+  // remove objects older than the specified duration
+  const rclcpp::Time old_limit = time - rclcpp::Duration::from_seconds(duration);
   for (const auto & object : objects_que_) {
-    // Skip if the object is older than the specified duration
     const rclcpp::Time object_time = rclcpp::Time(object.header.stamp);
-    const double time_diff = (object_time - time).seconds();
-    if (time_diff > duration) {
-      continue;
+    if (object_time < old_limit) {
+      objects_que_.pop_front();
+    } else {
+      break;
     }
+  }
+
+  for (const auto & object : objects_que_) {
+    // Skip if the object is older than the specified duration
+    const rclcpp::Time object_time = rclcpp::Time(object.header.stamp);
+    const double time_diff = (time - object_time).seconds();
     // Add the object if the object is older than the specified time
     if (time_diff >= 0.0) {
       objects.push_back(object);
+      // remove the object from the queue
+      objects_que_.pop_front();
     }
   }
+  RCLCPP_INFO(
+    node_.get_logger(), "InputStream::getObjectsOlderThan %s gives %zu objects", long_name_.c_str(),
+    objects.size());
+
+  // // message to show the older limit, time, old objects in the queue, latest object in the queue
+  // if (objects_que_.empty()) {
+  //   RCLCPP_INFO(
+  //     node_.get_logger(), "InputStream::getObjectsOlderThan %s empty queue", long_name_.c_str());
+  //   return;
+  // }
+  // RCLCPP_INFO(
+  //   node_.get_logger(), "InputStream::getObjectsOlderThan %s older limit %f, time %f, %u, %u",
+  //   long_name_.c_str(), old_limit.seconds(), time.seconds(),
+  //   objects_que_.front().header.stamp.sec, objects_que_.back().header.stamp.sec);
 }
 
 InputManager::InputManager(rclcpp::Node & node) : node_(node)
@@ -132,7 +156,7 @@ void InputManager::init(
       long_names.at(i).c_str(), input_topics.at(i).c_str());
     std::function<void(
       const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)>
-      func = std::bind(&InputStream::setObjects, input_stream, std::placeholders::_1);
+      func = std::bind(&InputStream::setObjects, input_streams_.at(i), std::placeholders::_1);
     sub_objects_array_.at(i) =
       node_.create_subscription<autoware_auto_perception_msgs::msg::DetectedObjects>(
         input_topics.at(i), rclcpp::QoS{1}, func);

From 9ebaf6dee1b3165ff90dc57b44e9dbd9e7aed3b4 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 15 Apr 2024 13:53:13 +0900
Subject: [PATCH 13/67] fix: get object time argument revise

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../processor/input_manager.hpp               | 16 +++-
 .../src/processor/input_manager.cpp           | 74 ++++++++++---------
 2 files changed, 53 insertions(+), 37 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 4afcf2a817675..6ebaba48a79c8 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -35,9 +35,23 @@ class InputStream
   void setObjects(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
 
   void getObjectsOlderThan(
-    const rclcpp::Time & time, const double duration,
+    const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
     std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects);
 
+  void getNames(std::string & long_name, std::string & short_name)
+  {
+    long_name = long_name_;
+    short_name = short_name_;
+  }
+  void getTimeStatistics(
+    double & latency_mean, double & latency_var, double & interval_mean, double & interval_var)
+  {
+    latency_mean = latency_mean_;
+    latency_var = latency_var_;
+    interval_mean = interval_mean_;
+    interval_var = interval_var_;
+  }
+
 private:
   rclcpp::Node & node_;
 
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 928b3874b4b8a..dc459bdd344b2 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -83,28 +83,24 @@ void InputStream::setObjects(
 }
 
 void InputStream::getObjectsOlderThan(
-  const rclcpp::Time & time, const double duration,
+  const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
 {
   objects.clear();
 
-  // remove objects older than the specified duration
-  const rclcpp::Time old_limit = time - rclcpp::Duration::from_seconds(duration);
+  assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds());
+
   for (const auto & object : objects_que_) {
     const rclcpp::Time object_time = rclcpp::Time(object.header.stamp);
-    if (object_time < old_limit) {
+
+    // remove objects older than the specified duration
+    if (object_time < object_oldest_time) {
       objects_que_.pop_front();
-    } else {
-      break;
+      continue;
     }
-  }
 
-  for (const auto & object : objects_que_) {
-    // Skip if the object is older than the specified duration
-    const rclcpp::Time object_time = rclcpp::Time(object.header.stamp);
-    const double time_diff = (time - object_time).seconds();
-    // Add the object if the object is older than the specified time
-    if (time_diff >= 0.0) {
+    // Add the object if the object is older than the specified latest time
+    if (object_latest_time >= object_time) {
       objects.push_back(object);
       // remove the object from the queue
       objects_que_.pop_front();
@@ -113,17 +109,6 @@ void InputStream::getObjectsOlderThan(
   RCLCPP_INFO(
     node_.get_logger(), "InputStream::getObjectsOlderThan %s gives %zu objects", long_name_.c_str(),
     objects.size());
-
-  // // message to show the older limit, time, old objects in the queue, latest object in the queue
-  // if (objects_que_.empty()) {
-  //   RCLCPP_INFO(
-  //     node_.get_logger(), "InputStream::getObjectsOlderThan %s empty queue", long_name_.c_str());
-  //   return;
-  // }
-  // RCLCPP_INFO(
-  //   node_.get_logger(), "InputStream::getObjectsOlderThan %s older limit %f, time %f, %u, %u",
-  //   long_name_.c_str(), old_limit.seconds(), time.seconds(),
-  //   objects_que_.front().header.stamp.sec, objects_que_.back().header.stamp.sec);
 }
 
 InputManager::InputManager(rclcpp::Node & node) : node_(node)
@@ -134,12 +119,12 @@ void InputManager::init(
   const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names,
   const std::vector<std::string> & short_names)
 {
-  input_size_ = input_topics.size();
-
   // Check input sizes
-  RCLCPP_INFO(
-    node_.get_logger(), "InputManager::init Initializing input manager with %zu input streams",
-    input_size_);
+  input_size_ = input_topics.size();
+  if (input_size_ == 0) {
+    RCLCPP_ERROR(node_.get_logger(), "InputManager::init No input streams");
+    return;
+  }
   assert(input_size_ == long_names.size());
   assert(input_size_ == short_names.size());
 
@@ -162,29 +147,46 @@ void InputManager::init(
         input_topics.at(i), rclcpp::QoS{1}, func);
   }
 
-  if (input_size_ > 0) {
-    is_initialized_ = true;
-  }
+  is_initialized_ = true;
 }
 
 bool InputManager::getObjects(
   const rclcpp::Time & now,
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
 {
-  RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Getting objects from input streams");
+  if (!is_initialized_) {
+    RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized");
+    return false;
+  }
 
+  RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Getting objects from input streams");
   objects.clear();
 
+  // Get the streams statistics
+  std::string long_name, short_name;
+  double latency_mean, latency_var, interval_mean, interval_var;
+  for (const auto & input_stream : input_streams_) {
+    input_stream->getNames(long_name, short_name);
+    input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var);
+    RCLCPP_INFO(
+      node_.get_logger(),
+      "InputManager::getObjects %s: latency mean: %f, var: %f, interval mean: "
+      "%f, var: %f",
+      long_name.c_str(), latency_mean, latency_var, interval_mean, interval_var);
+  }
+
   // Get proper latency
-  constexpr double target_latency = 0.15;   // [s], measurement to tracking latency, as much as the
+  constexpr double target_latency = 0.11;   // [s], measurement to tracking latency, as much as the
                                             // detector latency, the less total latency
   constexpr double acceptable_band = 0.15;  // [s], acceptable band from the target latency
-  rclcpp::Time time = now - rclcpp::Duration::from_seconds(target_latency);
+  const rclcpp::Time object_latest_time = now - rclcpp::Duration::from_seconds(target_latency);
+  const rclcpp::Time object_oldest_time =
+    object_latest_time - rclcpp::Duration::from_seconds(acceptable_band);
 
   // Get objects from all input streams
   for (const auto & input_stream : input_streams_) {
     std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_tmp;
-    input_stream->getObjectsOlderThan(time, acceptable_band, objects_tmp);
+    input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_tmp);
     objects.insert(objects.end(), objects_tmp.begin(), objects_tmp.end());
   }
 

From c496a25ca3e2ae41266e12e4e50e652e3ee1c4f2 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 15 Apr 2024 16:42:59 +0900
Subject: [PATCH 14/67] feat: back to periodic publish, analyze input latency
 and timings

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_core.hpp             |  7 --
 .../processor/input_manager.hpp               | 12 ++-
 .../src/multi_object_tracker_core.cpp         | 66 +++------------
 .../src/processor/input_manager.cpp           | 81 +++++++++++++++----
 4 files changed, 88 insertions(+), 78 deletions(-)

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
index e3a2864b1faaf..69c707526eb12 100644
--- 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
@@ -85,11 +85,6 @@ class MultiObjectTracker : public rclcpp::Node
   std::unique_ptr<DataAssociation> data_association_;
   std::unique_ptr<TrackerProcessor> processor_;
 
-  // inputs
-  rclcpp::Subscription<DetectedObjects>::SharedPtr sub_objects_{};
-  std::vector<rclcpp::Subscription<DetectedObjects>::SharedPtr> sub_objects_array_{};
-  rclcpp::Time last_measurement_time_;
-
   // input manager
   std::unique_ptr<InputManager> input_manager_;
 
@@ -98,8 +93,6 @@ class MultiObjectTracker : public rclcpp::Node
   std::vector<std::pair<rclcpp::Time, DetectedObjects>> objects_data_{};
 
   // callback functions
-  void onData(const DetectedObjects::ConstSharedPtr msg, const size_t array_number);
-  void onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg);
   void onTimer();
 
   // publish processes
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 6ebaba48a79c8..5047ffbd21dac 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -43,8 +43,12 @@ class InputStream
     long_name = long_name_;
     short_name = short_name_;
   }
+
+  size_t getObjectsCount() const { return objects_que_.size(); }
+
   void getTimeStatistics(
-    double & latency_mean, double & latency_var, double & interval_mean, double & interval_var)
+    double & latency_mean, double & latency_var, double & interval_mean,
+    double & interval_var) const
   {
     latency_mean = latency_mean_;
     latency_var = latency_var_;
@@ -52,6 +56,9 @@ class InputStream
     interval_var = interval_var_;
   }
 
+  bool getTimestamps(
+    rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const;
+
 private:
   rclcpp::Node & node_;
 
@@ -82,6 +89,8 @@ class InputManager
     const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names,
     const std::vector<std::string> & short_names);
 
+  bool isInputsReady() const;
+
   bool getObjects(
     const rclcpp::Time & now,
     std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects);
@@ -92,6 +101,7 @@ class InputManager
     sub_objects_array_{};
 
   bool is_initialized_{false};
+  rclcpp::Time latest_object_time_;
 
   size_t input_size_;
   std::vector<std::shared_ptr<InputStream>> input_streams_;
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 371a6d0996067..59b00f59d884c 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -87,9 +87,6 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   std::vector<std::string> input_names_long = get_parameter("input_names_long").as_string_array();
   std::vector<std::string> input_names_short = get_parameter("input_names_short").as_string_array();
 
-  // Initialize the last measurement time
-  last_measurement_time_ = this->now();
-
   // ROS interface - Publisher
   tracked_objects_pub_ = create_publisher<TrackedObjects>("output", rclcpp::QoS{1});
 
@@ -120,8 +117,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   // If the delay compensation is enabled, the timer is used to publish the output at the correct
   // time.
   if (enable_delay_compensation) {
-    publisher_period_ = 1.0 / publish_rate;    // [s]
-    constexpr double timer_multiplier = 20.0;  // 20 times frequent for publish timing check
+    publisher_period_ = 1.0 / publish_rate;   // [s]
+    constexpr double timer_multiplier = 1.0;  // 20 times frequent for publish timing check
     const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period();
     publish_timer_ = rclcpp::create_timer(
       this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this));
@@ -168,62 +165,19 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
-void MultiObjectTracker::onData(
-  const DetectedObjects::ConstSharedPtr msg, const size_t array_number)
-{
-  std::pair<rclcpp::Time, DetectedObjects> pair = std::make_pair(msg->header.stamp, *msg);
-  objects_data_.push_back(pair);
-
-  // debug message
-  RCLCPP_INFO(
-    get_logger(), "Received data from topic %s", input_topic_names_.at(array_number).c_str());
-  if (last_measurement_time_ > msg->header.stamp) {
-    const double delta_time = (last_measurement_time_ - msg->header.stamp).seconds() * 1e3;
-    RCLCPP_INFO(
-      get_logger(), "Received data is older than the last measurement time by %f ms", delta_time);
-    return;
-  }
-  if (last_measurement_time_ < msg->header.stamp) {
-    last_measurement_time_ = msg->header.stamp;
-  }
-
-  // onMeasurement(msg);
-}
-
-void MultiObjectTracker::onMeasurement(const DetectedObjects::ConstSharedPtr input_objects_msg)
-{
-  // Get the time of the measurement
-  const rclcpp::Time measurement_time =
-    rclcpp::Time(input_objects_msg->header.stamp, this->now().get_clock_type());
-
-  debugger_->startMeasurementTime(this->now(), measurement_time);
-  runProcess(*input_objects_msg);
-  debugger_->endMeasurementTime(this->now());
-
-  // Publish objects if the timer is not enabled
-  if (publish_timer_ == nullptr) {
-    // Publish if the delay compensation is disabled
-    publish(measurement_time);
-  } else {
-    // Publish if the next publish time is close
-    const double minimum_publish_interval = publisher_period_ * 0.70;  // 70% of the period
-    if ((this->now() - last_published_time_).seconds() > minimum_publish_interval) {
-      checkAndPublish(this->now());
-    }
-  }
-}
-
 void MultiObjectTracker::onTimer()
 {
+  // if (!input_manager_->isInputsReady()) return;
+
   const rclcpp::Time current_time = this->now();
 
-  // Check the publish period
-  const auto elapsed_time = (current_time - last_published_time_).seconds();
-  // If the elapsed time is over the period, publish objects with prediction
-  constexpr double maximum_latency_ratio = 1.11;  // 11% margin
-  const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio;
+  // // Check the publish period
+  // const auto elapsed_time = (current_time - last_published_time_).seconds();
+  // // If the elapsed time is over the period, publish objects with prediction
+  // constexpr double maximum_latency_ratio = 1.11;  // 11% margin
+  // const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio;
 
-  if (elapsed_time < maximum_publish_latency) return;
+  // if (elapsed_time < maximum_publish_latency) return;
 
   // get objects from the input manager and run process
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_data;
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index dc459bdd344b2..85a911774d0bc 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -38,15 +38,29 @@ void InputStream::init(
   latency_var_ = 0.0;
   interval_mean_ = 1 / expected_rate_;
   interval_var_ = 0.0;
+
+  latest_measurement_time_ = node_.now();
+  latest_message_time_ = node_.now();
+}
+
+bool InputStream::getTimestamps(
+  rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const
+{
+  if (!is_time_initialized_) {
+    return false;
+  }
+  latest_measurement_time = latest_measurement_time_;
+  latest_message_time = latest_message_time_;
+  return true;
 }
 
 void InputStream::setObjects(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)
 {
-  // debug message
-  RCLCPP_INFO(
-    node_.get_logger(), "InputStream::setObjects Received %s message from %s at %d",
-    long_name_.c_str(), input_topic_.c_str(), msg->header.stamp.sec);
+  // // debug message
+  // RCLCPP_INFO(
+  //   node_.get_logger(), "InputStream::setObjects Received %s message from %s at %d.%d",
+  //   long_name_.c_str(), input_topic_.c_str(), msg->header.stamp.sec, msg->header.stamp.nanosec);
 
   const autoware_auto_perception_msgs::msg::DetectedObjects object = *msg;
   objects_que_.push_back(object);
@@ -54,7 +68,7 @@ void InputStream::setObjects(
     objects_que_.pop_front();
   }
 
-  RCLCPP_INFO(node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size());
+  // RCLCPP_INFO(node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size());
 
   // Filter parameters
   constexpr double gain = 0.05;
@@ -113,6 +127,7 @@ void InputStream::getObjectsOlderThan(
 
 InputManager::InputManager(rclcpp::Node & node) : node_(node)
 {
+  latest_object_time_ = node_.now();
 }
 
 void InputManager::init(
@@ -150,6 +165,18 @@ void InputManager::init(
   is_initialized_ = true;
 }
 
+bool InputManager::isInputsReady() const
+{
+  if (!is_initialized_) {
+    RCLCPP_INFO(
+      node_.get_logger(), "InputManager::isMajorInputReady Input manager is not initialized");
+    return false;
+  }
+
+  // Check if the major input stream (index of 0) is ready
+  return input_streams_.at(0)->getObjectsCount() > 0;
+}
+
 bool InputManager::getObjects(
   const rclcpp::Time & now,
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
@@ -162,26 +189,38 @@ bool InputManager::getObjects(
   RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Getting objects from input streams");
   objects.clear();
 
-  // Get the streams statistics
+  // ANALYSIS: Get the streams statistics
   std::string long_name, short_name;
   double latency_mean, latency_var, interval_mean, interval_var;
+  rclcpp::Time latest_measurement_time, latest_message_time;
   for (const auto & input_stream : input_streams_) {
     input_stream->getNames(long_name, short_name);
     input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var);
+    if (!input_stream->getTimestamps(latest_measurement_time, latest_message_time)) {
+      continue;
+    }
+    double latency_message = (now - latest_message_time).seconds();
+    double latency_measurement = (now - latest_measurement_time).seconds();
     RCLCPP_INFO(
       node_.get_logger(),
-      "InputManager::getObjects %s: latency mean: %f, var: %f, interval mean: "
-      "%f, var: %f",
-      long_name.c_str(), latency_mean, latency_var, interval_mean, interval_var);
+      "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: "
+      "%f, std: %f, latest measurement latency: %f, latest message latency: %f",
+      long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean,
+      std::sqrt(interval_var), latency_measurement, latency_message);
   }
 
   // Get proper latency
-  constexpr double target_latency = 0.11;   // [s], measurement to tracking latency, as much as the
-                                            // detector latency, the less total latency
-  constexpr double acceptable_band = 0.15;  // [s], acceptable band from the target latency
+  constexpr double target_latency = 0.15;  // [s], measurement to tracking latency, as much as the
+                                           // detector latency, the less total latency
+  constexpr double acceptable_latency =
+    0.35;  // [s], acceptable band from the target latency, larger than the target latency
   const rclcpp::Time object_latest_time = now - rclcpp::Duration::from_seconds(target_latency);
-  const rclcpp::Time object_oldest_time =
-    object_latest_time - rclcpp::Duration::from_seconds(acceptable_band);
+  rclcpp::Time object_oldest_time = now - rclcpp::Duration::from_seconds(acceptable_latency);
+
+  // if the object_oldest_time is older than the latest object time, set it to the latest object
+  // time
+  object_oldest_time =
+    object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_;
 
   // Get objects from all input streams
   for (const auto & input_stream : input_streams_) {
@@ -199,9 +238,23 @@ bool InputManager::getObjects(
       return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0;
     });
 
+  // ANALYSIS: Obtained object time range
+  if (!objects.empty()) {
+    rclcpp::Time oldest_time(objects.front().header.stamp);
+    rclcpp::Time latest_time(objects.back().header.stamp);
+    RCLCPP_INFO(
+      node_.get_logger(), "InputManager::getObjects Object time range: %f - %f",
+      (now - latest_time).seconds(), (now - oldest_time).seconds());
+  }
+
   RCLCPP_INFO(
     node_.get_logger(), "InputManager::getObjects Got %zu objects from input streams",
     objects.size());
 
+  // Update the latest object time
+  if (!objects.empty()) {
+    latest_object_time_ = rclcpp::Time(objects.back().header.stamp);
+  }
+
   return !objects.empty();
 }

From 969483c902dfa6b50dad559f01362418fd40d15a Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 15 Apr 2024 17:27:27 +0900
Subject: [PATCH 15/67] fix: enable timing debugger

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../src/multi_object_tracker_core.cpp         | 15 +++++++++--
 .../src/processor/input_manager.cpp           | 26 +++++++++----------
 2 files changed, 26 insertions(+), 15 deletions(-)

diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 59b00f59d884c..9714fba08029f 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -181,12 +181,23 @@ void MultiObjectTracker::onTimer()
 
   // get objects from the input manager and run process
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_data;
-  if (input_manager_->getObjects(current_time, objects_data)) {
+  const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data);
+  if (is_objects_ready) {
+    debugger_->startMeasurementTime(this->now(), rclcpp::Time(objects_data.front().header.stamp));
+
     for (const auto & objects : objects_data) {
       runProcess(objects);
     }
-  }
 
+    // for debug
+    rclcpp::Time oldest_time(objects_data.front().header.stamp);
+    rclcpp::Time latest_time(objects_data.back().header.stamp);
+    RCLCPP_INFO(
+      this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f",
+      (current_time - latest_time).seconds(), (current_time - oldest_time).seconds());
+
+    debugger_->endMeasurementTime(this->now());
+  }
   // Publish
   checkAndPublish(current_time);
 }
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 85a911774d0bc..d8bcc85e49d52 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -100,7 +100,7 @@ void InputStream::getObjectsOlderThan(
   const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
 {
-  objects.clear();
+  // objects.clear();
 
   assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds());
 
@@ -186,7 +186,6 @@ bool InputManager::getObjects(
     return false;
   }
 
-  RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Getting objects from input streams");
   objects.clear();
 
   // ANALYSIS: Get the streams statistics
@@ -224,9 +223,10 @@ bool InputManager::getObjects(
 
   // Get objects from all input streams
   for (const auto & input_stream : input_streams_) {
-    std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_tmp;
-    input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_tmp);
-    objects.insert(objects.end(), objects_tmp.begin(), objects_tmp.end());
+    // std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_tmp;
+    // input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_tmp);
+    // objects.insert(objects.end(), objects_tmp.begin(), objects_tmp.end());
+    input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects);
   }
 
   // Sort objects by timestamp
@@ -238,14 +238,14 @@ bool InputManager::getObjects(
       return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0;
     });
 
-  // ANALYSIS: Obtained object time range
-  if (!objects.empty()) {
-    rclcpp::Time oldest_time(objects.front().header.stamp);
-    rclcpp::Time latest_time(objects.back().header.stamp);
-    RCLCPP_INFO(
-      node_.get_logger(), "InputManager::getObjects Object time range: %f - %f",
-      (now - latest_time).seconds(), (now - oldest_time).seconds());
-  }
+  // // ANALYSIS: Obtained object time range
+  // if (!objects.empty()) {
+  //   rclcpp::Time oldest_time(objects.front().header.stamp);
+  //   rclcpp::Time latest_time(objects.back().header.stamp);
+  //   RCLCPP_INFO(
+  //     node_.get_logger(), "InputManager::getObjects Object time range: %f - %f",
+  //     (now - latest_time).seconds(), (now - oldest_time).seconds());
+  // }
 
   RCLCPP_INFO(
     node_.get_logger(), "InputManager::getObjects Got %zu objects from input streams",

From ceaacc3c176237d47ef71cfa06cdc4823c8d53f4 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 15 Apr 2024 18:28:46 +0900
Subject: [PATCH 16/67] fix: separate object interval function

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../processor/input_manager.hpp               |  2 +
 .../src/multi_object_tracker_core.cpp         |  3 +-
 .../src/processor/input_manager.cpp           | 90 +++++++++----------
 3 files changed, 48 insertions(+), 47 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 5047ffbd21dac..249b6bcaadb19 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -90,6 +90,8 @@ class InputManager
     const std::vector<std::string> & short_names);
 
   bool isInputsReady() const;
+  void getObjectTimeInterval(
+    const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time);
 
   bool getObjects(
     const rclcpp::Time & now,
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 9714fba08029f..9d3a701e26da5 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -118,7 +118,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   // time.
   if (enable_delay_compensation) {
     publisher_period_ = 1.0 / publish_rate;   // [s]
-    constexpr double timer_multiplier = 1.0;  // 20 times frequent for publish timing check
+    constexpr double timer_multiplier = 1.0;  // 5 times frequent for publish timing check
     const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period();
     publish_timer_ = rclcpp::create_timer(
       this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this));
@@ -167,6 +167,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
 
 void MultiObjectTracker::onTimer()
 {
+  // // Check the input manager
   // if (!input_manager_->isInputsReady()) return;
 
   const rclcpp::Time current_time = this->now();
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index d8bcc85e49d52..b9b5f25ea4f96 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -34,7 +34,7 @@ void InputStream::init(
 
   // Initialize latency statistics
   expected_rate_ = 10.0;
-  latency_mean_ = 0.10;  // [s]
+  latency_mean_ = 0.18;  // [s]
   latency_var_ = 0.0;
   interval_mean_ = 1 / expected_rate_;
   interval_var_ = 0.0;
@@ -100,8 +100,6 @@ void InputStream::getObjectsOlderThan(
   const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
 {
-  // objects.clear();
-
   assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds());
 
   for (const auto & object : objects_que_) {
@@ -177,55 +175,64 @@ bool InputManager::isInputsReady() const
   return input_streams_.at(0)->getObjectsCount() > 0;
 }
 
-bool InputManager::getObjects(
-  const rclcpp::Time & now,
-  std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
+void InputManager::getObjectTimeInterval(
+  const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time)
 {
-  if (!is_initialized_) {
-    RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized");
-    return false;
-  }
-
-  objects.clear();
-
-  // ANALYSIS: Get the streams statistics
-  std::string long_name, short_name;
-  double latency_mean, latency_var, interval_mean, interval_var;
-  rclcpp::Time latest_measurement_time, latest_message_time;
-  for (const auto & input_stream : input_streams_) {
-    input_stream->getNames(long_name, short_name);
-    input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var);
-    if (!input_stream->getTimestamps(latest_measurement_time, latest_message_time)) {
-      continue;
+  {
+    // ANALYSIS: Get the streams statistics
+    std::string long_name, short_name;
+    double latency_mean, latency_var, interval_mean, interval_var;
+    rclcpp::Time latest_measurement_time, latest_message_time;
+    for (const auto & input_stream : input_streams_) {
+      input_stream->getNames(long_name, short_name);
+      input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var);
+      if (!input_stream->getTimestamps(latest_measurement_time, latest_message_time)) {
+        continue;
+      }
+      double latency_message = (now - latest_message_time).seconds();
+      double latency_measurement = (now - latest_measurement_time).seconds();
+      RCLCPP_INFO(
+        node_.get_logger(),
+        "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: "
+        "%f, std: %f, latest measurement latency: %f, latest message latency: %f",
+        long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean,
+        std::sqrt(interval_var), latency_measurement, latency_message);
     }
-    double latency_message = (now - latest_message_time).seconds();
-    double latency_measurement = (now - latest_measurement_time).seconds();
-    RCLCPP_INFO(
-      node_.get_logger(),
-      "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: "
-      "%f, std: %f, latest measurement latency: %f, latest message latency: %f",
-      long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean,
-      std::sqrt(interval_var), latency_measurement, latency_message);
   }
 
   // Get proper latency
-  constexpr double target_latency = 0.15;  // [s], measurement to tracking latency, as much as the
-                                           // detector latency, the less total latency
+  constexpr double target_latency = 0.15;  // [s], measurement to tracking latency
+                                           // process latency of a main detection + margin
+
   constexpr double acceptable_latency =
     0.35;  // [s], acceptable band from the target latency, larger than the target latency
-  const rclcpp::Time object_latest_time = now - rclcpp::Duration::from_seconds(target_latency);
-  rclcpp::Time object_oldest_time = now - rclcpp::Duration::from_seconds(acceptable_latency);
+  object_latest_time = now - rclcpp::Duration::from_seconds(target_latency);
+  object_oldest_time = now - rclcpp::Duration::from_seconds(acceptable_latency);
 
   // if the object_oldest_time is older than the latest object time, set it to the latest object
   // time
   object_oldest_time =
     object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_;
+}
+
+bool InputManager::getObjects(
+  const rclcpp::Time & now,
+  std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
+{
+  if (!is_initialized_) {
+    RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized");
+    return false;
+  }
+
+  // Clear the objects
+  objects.clear();
+
+  // Get the time interval for the objects
+  rclcpp::Time object_latest_time, object_oldest_time;
+  getObjectTimeInterval(now, object_latest_time, object_oldest_time);
 
   // Get objects from all input streams
   for (const auto & input_stream : input_streams_) {
-    // std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_tmp;
-    // input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_tmp);
-    // objects.insert(objects.end(), objects_tmp.begin(), objects_tmp.end());
     input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects);
   }
 
@@ -238,15 +245,6 @@ bool InputManager::getObjects(
       return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0;
     });
 
-  // // ANALYSIS: Obtained object time range
-  // if (!objects.empty()) {
-  //   rclcpp::Time oldest_time(objects.front().header.stamp);
-  //   rclcpp::Time latest_time(objects.back().header.stamp);
-  //   RCLCPP_INFO(
-  //     node_.get_logger(), "InputManager::getObjects Object time range: %f - %f",
-  //     (now - latest_time).seconds(), (now - oldest_time).seconds());
-  // }
-
   RCLCPP_INFO(
     node_.get_logger(), "InputManager::getObjects Got %zu objects from input streams",
     objects.size());

From c556460e7ccb3284ac23ec7bc0dd7e4ece03db6a Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 15 Apr 2024 19:24:45 +0900
Subject: [PATCH 17/67] feat: prepare message triggered process

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_core.hpp             |  1 +
 .../processor/input_manager.hpp               |  2 +-
 .../src/multi_object_tracker_core.cpp         | 23 ++++++++++++++++++-
 .../src/processor/input_manager.cpp           |  8 +++----
 4 files changed, 28 insertions(+), 6 deletions(-)

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
index 69c707526eb12..fd640a073e038 100644
--- 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
@@ -94,6 +94,7 @@ class MultiObjectTracker : public rclcpp::Node
 
   // callback functions
   void onTimer();
+  void onMessage(const std::vector<DetectedObjects> & objects_data);
 
   // publish processes
   void runProcess(const DetectedObjects & input_objects_msg);
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 249b6bcaadb19..90148b19c5cf6 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -32,7 +32,7 @@ class InputStream
   void init(
     const std::string & input_topic, const std::string & long_name, const std::string & short_name);
 
-  void setObjects(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
+  void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
 
   void getObjectsOlderThan(
     const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 9d3a701e26da5..01136c15a4f4f 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -165,6 +165,28 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
+void MultiObjectTracker::onMessage(const std::vector<DetectedObjects> & objects_data)
+{
+  const rclcpp::Time current_time = this->now();
+  debugger_->startMeasurementTime(this->now(), rclcpp::Time(objects_data.front().header.stamp));
+
+  // run process for each DetectedObjects
+  for (const auto & objects : objects_data) {
+    runProcess(objects);
+  }
+
+  // for debug
+  rclcpp::Time oldest_time(objects_data.front().header.stamp);
+  rclcpp::Time latest_time(objects_data.back().header.stamp);
+  RCLCPP_INFO(
+    this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f",
+    (current_time - latest_time).seconds(), (current_time - oldest_time).seconds());
+
+  debugger_->endMeasurementTime(this->now());
+
+  // Publish
+}
+
 void MultiObjectTracker::onTimer()
 {
   // // Check the input manager
@@ -177,7 +199,6 @@ void MultiObjectTracker::onTimer()
   // // If the elapsed time is over the period, publish objects with prediction
   // constexpr double maximum_latency_ratio = 1.11;  // 11% margin
   // const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio;
-
   // if (elapsed_time < maximum_publish_latency) return;
 
   // get objects from the input manager and run process
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index b9b5f25ea4f96..f0c5655b05c6e 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -54,12 +54,12 @@ bool InputStream::getTimestamps(
   return true;
 }
 
-void InputStream::setObjects(
+void InputStream::onMessage(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)
 {
   // // debug message
   // RCLCPP_INFO(
-  //   node_.get_logger(), "InputStream::setObjects Received %s message from %s at %d.%d",
+  //   node_.get_logger(), "InputStream::onMessage Received %s message from %s at %d.%d",
   //   long_name_.c_str(), input_topic_.c_str(), msg->header.stamp.sec, msg->header.stamp.nanosec);
 
   const autoware_auto_perception_msgs::msg::DetectedObjects object = *msg;
@@ -68,7 +68,7 @@ void InputStream::setObjects(
     objects_que_.pop_front();
   }
 
-  // RCLCPP_INFO(node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size());
+  // RCLCPP_INFO(node_.get_logger(), "InputStream::onMessage Que size: %zu", objects_que_.size());
 
   // Filter parameters
   constexpr double gain = 0.05;
@@ -154,7 +154,7 @@ void InputManager::init(
       long_names.at(i).c_str(), input_topics.at(i).c_str());
     std::function<void(
       const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)>
-      func = std::bind(&InputStream::setObjects, input_streams_.at(i), std::placeholders::_1);
+      func = std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1);
     sub_objects_array_.at(i) =
       node_.create_subscription<autoware_auto_perception_msgs::msg::DetectedObjects>(
         input_topics.at(i), rclcpp::QoS{1}, func);

From 4afab3fec1b56a1fcd75292586d914031e0c5a21 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Tue, 16 Apr 2024 12:14:11 +0900
Subject: [PATCH 18/67] feat: trigger tracker by main message arrive

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_core.hpp             |  1 +
 .../processor/input_manager.hpp               | 24 +++--
 .../src/multi_object_tracker_core.cpp         | 92 ++++++++++---------
 .../src/processor/input_manager.cpp           | 26 ++++--
 4 files changed, 84 insertions(+), 59 deletions(-)

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
index fd640a073e038..7ac0cbb303799 100644
--- 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
@@ -94,6 +94,7 @@ class MultiObjectTracker : public rclcpp::Node
 
   // callback functions
   void onTimer();
+  void onTrigger();
   void onMessage(const std::vector<DetectedObjects> & objects_data);
 
   // publish processes
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 90148b19c5cf6..4626a372ce5ea 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -27,25 +27,28 @@
 class InputStream
 {
 public:
-  explicit InputStream(rclcpp::Node & node);
+  explicit InputStream(rclcpp::Node & node, size_t & index);
 
   void init(
     const std::string & input_topic, const std::string & long_name, const std::string & short_name);
 
+  void setQueueSize(size_t que_size) { que_size_ = que_size; }
+  void setTriggerFunction(std::function<void(const size_t &)> func_trigger)
+  {
+    func_trigger_ = func_trigger;
+  }
+
   void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
 
   void getObjectsOlderThan(
     const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
     std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects);
-
   void getNames(std::string & long_name, std::string & short_name)
   {
     long_name = long_name_;
     short_name = short_name_;
   }
-
   size_t getObjectsCount() const { return objects_que_.size(); }
-
   void getTimeStatistics(
     double & latency_mean, double & latency_var, double & interval_mean,
     double & interval_var) const
@@ -55,20 +58,22 @@ class InputStream
     interval_mean = interval_mean_;
     interval_var = interval_var_;
   }
-
   bool getTimestamps(
     rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const;
 
 private:
   rclcpp::Node & node_;
+  size_t index_;
 
   std::string input_topic_;
   std::string long_name_;
   std::string short_name_;
 
-  size_t que_size_{50};
+  size_t que_size_{30};
   std::deque<autoware_auto_perception_msgs::msg::DetectedObjects> objects_que_;
 
+  std::function<void(const size_t &)> func_trigger_;
+
   bool is_time_initialized_{false};
   double expected_rate_;
   double latency_mean_;
@@ -88,11 +93,12 @@ class InputManager
   void init(
     const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names,
     const std::vector<std::string> & short_names);
+  void setTriggerFunction(std::function<void()> func_trigger) { func_trigger_ = func_trigger; }
+
+  void onTrigger(const size_t & index) const;
 
-  bool isInputsReady() const;
   void getObjectTimeInterval(
     const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time);
-
   bool getObjects(
     const rclcpp::Time & now,
     std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects);
@@ -107,6 +113,8 @@ class InputManager
 
   size_t input_size_;
   std::vector<std::shared_ptr<InputStream>> input_streams_;
+
+  std::function<void()> func_trigger_;
 };
 
 #endif  // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 01136c15a4f4f..f4f7a30de7aae 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -107,6 +107,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   // Initialize input manager
   input_manager_ = std::make_unique<InputManager>(*this);
   input_manager_->init(input_topic_names, input_names_long, input_names_short);
+  // Set trigger function
+  input_manager_->setTriggerFunction(std::bind(&MultiObjectTracker::onTrigger, this));
 
   // Create tf timer
   auto cti = std::make_shared<tf2_ros::CreateTimerROS>(
@@ -117,8 +119,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   // If the delay compensation is enabled, the timer is used to publish the output at the correct
   // time.
   if (enable_delay_compensation) {
-    publisher_period_ = 1.0 / publish_rate;   // [s]
-    constexpr double timer_multiplier = 1.0;  // 5 times frequent for publish timing check
+    publisher_period_ = 1.0 / publish_rate;    // [s]
+    constexpr double timer_multiplier = 20.0;  // 20 times frequent for publish timing check
     const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period();
     publish_timer_ = rclcpp::create_timer(
       this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this));
@@ -165,63 +167,71 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
-void MultiObjectTracker::onMessage(const std::vector<DetectedObjects> & objects_data)
+void MultiObjectTracker::onTrigger()
 {
   const rclcpp::Time current_time = this->now();
-  debugger_->startMeasurementTime(this->now(), rclcpp::Time(objects_data.front().header.stamp));
-
-  // run process for each DetectedObjects
-  for (const auto & objects : objects_data) {
-    runProcess(objects);
+  // get objects from the input manager and run process
+  std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_data;
+  const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data);
+  if (!is_objects_ready) return;
+
+  onMessage(objects_data);
+  const rclcpp::Time latest_time(objects_data.back().header.stamp);
+
+  // Publish objects if the timer is not enabled
+  if (publish_timer_ == nullptr) {
+    // if the delay compensation is disabled, publish the objects in the latest time
+    publish(latest_time);
+  } else {
+    // Publish if the next publish time is close
+    const double minimum_publish_interval = publisher_period_ * 0.70;  // 70% of the period
+    if ((current_time - last_published_time_).seconds() > minimum_publish_interval) {
+      checkAndPublish(current_time);
+    }
   }
-
-  // for debug
-  rclcpp::Time oldest_time(objects_data.front().header.stamp);
-  rclcpp::Time latest_time(objects_data.back().header.stamp);
-  RCLCPP_INFO(
-    this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f",
-    (current_time - latest_time).seconds(), (current_time - oldest_time).seconds());
-
-  debugger_->endMeasurementTime(this->now());
-
-  // Publish
 }
 
 void MultiObjectTracker::onTimer()
 {
-  // // Check the input manager
-  // if (!input_manager_->isInputsReady()) return;
-
   const rclcpp::Time current_time = this->now();
 
-  // // Check the publish period
-  // const auto elapsed_time = (current_time - last_published_time_).seconds();
-  // // If the elapsed time is over the period, publish objects with prediction
-  // constexpr double maximum_latency_ratio = 1.11;  // 11% margin
-  // const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio;
-  // if (elapsed_time < maximum_publish_latency) return;
+  // Check the publish period
+  const auto elapsed_time = (current_time - last_published_time_).seconds();
+  // If the elapsed time is over the period, publish objects with prediction
+  constexpr double maximum_latency_ratio = 1.11;  // 11% margin
+  const double maximum_publish_latency = publisher_period_ * maximum_latency_ratio;
+  if (elapsed_time < maximum_publish_latency) return;
 
   // get objects from the input manager and run process
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_data;
   const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data);
   if (is_objects_ready) {
-    debugger_->startMeasurementTime(this->now(), rclcpp::Time(objects_data.front().header.stamp));
+    onMessage(objects_data);
+  }
 
-    for (const auto & objects : objects_data) {
-      runProcess(objects);
-    }
+  // Publish
+  checkAndPublish(current_time);
+}
 
-    // for debug
-    rclcpp::Time oldest_time(objects_data.front().header.stamp);
-    rclcpp::Time latest_time(objects_data.back().header.stamp);
-    RCLCPP_INFO(
-      this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f",
-      (current_time - latest_time).seconds(), (current_time - oldest_time).seconds());
+void MultiObjectTracker::onMessage(const std::vector<DetectedObjects> & objects_data)
+{
+  const rclcpp::Time current_time = this->now();
 
-    debugger_->endMeasurementTime(this->now());
+  // process start
+  debugger_->startMeasurementTime(this->now(), rclcpp::Time(objects_data.front().header.stamp));
+  // run process for each DetectedObjects
+  for (const auto & objects : objects_data) {
+    runProcess(objects);
   }
-  // Publish
-  checkAndPublish(current_time);
+  // process end
+  debugger_->endMeasurementTime(this->now());
+
+  // for debug
+  const rclcpp::Time oldest_time(objects_data.front().header.stamp);
+  const rclcpp::Time latest_time(objects_data.back().header.stamp);
+  RCLCPP_INFO(
+    this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f",
+    (current_time - latest_time).seconds(), (current_time - oldest_time).seconds());
 }
 
 void MultiObjectTracker::runProcess(const DetectedObjects & input_objects_msg)
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index f0c5655b05c6e..25dbfd3edc41c 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -17,7 +17,7 @@
 #include <cassert>
 #include <functional>
 
-InputStream::InputStream(rclcpp::Node & node) : node_(node)
+InputStream::InputStream(rclcpp::Node & node, size_t & index) : node_(node), index_(index)
 {
 }
 
@@ -94,6 +94,11 @@ void InputStream::onMessage(
   latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency;
   const double latency_delta = latency - latency_mean_;
   latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta;
+
+  // trigger the function if it is set
+  if (func_trigger_) {
+    func_trigger_(index_);
+  }
 }
 
 void InputStream::getObjectsOlderThan(
@@ -144,8 +149,10 @@ void InputManager::init(
   sub_objects_array_.resize(input_size_);
 
   for (size_t i = 0; i < input_size_; i++) {
-    InputStream input_stream(node_);
+    InputStream input_stream(node_, i);
     input_stream.init(input_topics.at(i), long_names.at(i), short_names.at(i));
+    input_stream.setTriggerFunction(
+      std::bind(&InputManager::onTrigger, this, std::placeholders::_1));
     input_streams_.push_back(std::make_shared<InputStream>(input_stream));
 
     // Set subscription
@@ -163,16 +170,15 @@ void InputManager::init(
   is_initialized_ = true;
 }
 
-bool InputManager::isInputsReady() const
+void InputManager::onTrigger(const size_t & index) const
 {
-  if (!is_initialized_) {
-    RCLCPP_INFO(
-      node_.get_logger(), "InputManager::isMajorInputReady Input manager is not initialized");
-    return false;
-  }
+  // input stream index of 0 is the target(main) input stream
+  const size_t target_idx = 0;
 
-  // Check if the major input stream (index of 0) is ready
-  return input_streams_.at(0)->getObjectsCount() > 0;
+  // when the main stream triggers, call the trigger function
+  if (index == target_idx && func_trigger_) {
+    func_trigger_();
+  }
 }
 
 void InputManager::getObjectTimeInterval(

From 23a9df4e93945b322a3d24941088667968bdd372 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Tue, 16 Apr 2024 15:09:34 +0900
Subject: [PATCH 19/67] chore: clean-up, set namespace

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../processor/input_manager.hpp               | 19 ++++++----
 .../src/multi_object_tracker_core.cpp         |  4 +-
 .../src/processor/input_manager.cpp           | 38 +++++++++----------
 3 files changed, 32 insertions(+), 29 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 4626a372ce5ea..da19d5628be9c 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -20,10 +20,16 @@
 #include "autoware_auto_perception_msgs/msg/detected_objects.hpp"
 
 #include <deque>
+#include <functional>
 #include <memory>
 #include <string>
 #include <vector>
 
+namespace multi_object_tracker
+{
+using autoware_auto_perception_msgs::msg::DetectedObject;
+using autoware_auto_perception_msgs::msg::DetectedObjects;
+
 class InputStream
 {
 public:
@@ -42,7 +48,7 @@ class InputStream
 
   void getObjectsOlderThan(
     const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
-    std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects);
+    std::vector<DetectedObjects> & objects);
   void getNames(std::string & long_name, std::string & short_name)
   {
     long_name = long_name_;
@@ -70,7 +76,7 @@ class InputStream
   std::string short_name_;
 
   size_t que_size_{30};
-  std::deque<autoware_auto_perception_msgs::msg::DetectedObjects> objects_que_;
+  std::deque<DetectedObjects> objects_que_;
 
   std::function<void(const size_t &)> func_trigger_;
 
@@ -99,14 +105,11 @@ class InputManager
 
   void getObjectTimeInterval(
     const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time);
-  bool getObjects(
-    const rclcpp::Time & now,
-    std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects);
+  bool getObjects(const rclcpp::Time & now, std::vector<DetectedObjects> & objects);
 
 private:
   rclcpp::Node & node_;
-  std::vector<rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr>
-    sub_objects_array_{};
+  std::vector<rclcpp::Subscription<DetectedObjects>::SharedPtr> sub_objects_array_{};
 
   bool is_initialized_{false};
   rclcpp::Time latest_object_time_;
@@ -117,4 +120,6 @@ class InputManager
   std::function<void()> func_trigger_;
 };
 
+}  // namespace multi_object_tracker
+
 #endif  // MULTI_OBJECT_TRACKER__PROCESSOR__INPUT_MANAGER_HPP_
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index f4f7a30de7aae..f3a272acdbad7 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -171,7 +171,7 @@ void MultiObjectTracker::onTrigger()
 {
   const rclcpp::Time current_time = this->now();
   // get objects from the input manager and run process
-  std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_data;
+  std::vector<DetectedObjects> objects_data;
   const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data);
   if (!is_objects_ready) return;
 
@@ -203,7 +203,7 @@ void MultiObjectTracker::onTimer()
   if (elapsed_time < maximum_publish_latency) return;
 
   // get objects from the input manager and run process
-  std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_data;
+  std::vector<DetectedObjects> objects_data;
   const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data);
   if (is_objects_ready) {
     onMessage(objects_data);
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 25dbfd3edc41c..b9ed035f1e2c6 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -15,7 +15,9 @@
 #include "multi_object_tracker/processor/input_manager.hpp"
 
 #include <cassert>
-#include <functional>
+
+namespace multi_object_tracker
+{
 
 InputStream::InputStream(rclcpp::Node & node, size_t & index) : node_(node), index_(index)
 {
@@ -62,8 +64,8 @@ void InputStream::onMessage(
   //   node_.get_logger(), "InputStream::onMessage Received %s message from %s at %d.%d",
   //   long_name_.c_str(), input_topic_.c_str(), msg->header.stamp.sec, msg->header.stamp.nanosec);
 
-  const autoware_auto_perception_msgs::msg::DetectedObjects object = *msg;
-  objects_que_.push_back(object);
+  const DetectedObjects objects = *msg;
+  objects_que_.push_back(objects);
   if (objects_que_.size() > que_size_) {
     objects_que_.pop_front();
   }
@@ -84,7 +86,7 @@ void InputStream::onMessage(
 
   // Update time
   latest_message_time_ = now;
-  latest_measurement_time_ = object.header.stamp;
+  latest_measurement_time_ = objects.header.stamp;
   if (!is_time_initialized_) is_time_initialized_ = true;
 
   // Calculate latency
@@ -103,7 +105,7 @@ void InputStream::onMessage(
 
 void InputStream::getObjectsOlderThan(
   const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
-  std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
+  std::vector<DetectedObjects> & objects)
 {
   assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds());
 
@@ -123,9 +125,9 @@ void InputStream::getObjectsOlderThan(
       objects_que_.pop_front();
     }
   }
-  RCLCPP_INFO(
-    node_.get_logger(), "InputStream::getObjectsOlderThan %s gives %zu objects", long_name_.c_str(),
-    objects.size());
+  // RCLCPP_INFO(
+  //   node_.get_logger(), "InputStream::getObjectsOlderThan %s gives %zu objects",
+  //   long_name_.c_str(), objects.size());
 }
 
 InputManager::InputManager(rclcpp::Node & node) : node_(node)
@@ -159,12 +161,10 @@ void InputManager::init(
     RCLCPP_INFO(
       node_.get_logger(), "InputManager::init Initializing %s input stream from %s",
       long_names.at(i).c_str(), input_topics.at(i).c_str());
-    std::function<void(
-      const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)>
-      func = std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1);
+    std::function<void(const DetectedObjects::ConstSharedPtr msg)> func =
+      std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1);
     sub_objects_array_.at(i) =
-      node_.create_subscription<autoware_auto_perception_msgs::msg::DetectedObjects>(
-        input_topics.at(i), rclcpp::QoS{1}, func);
+      node_.create_subscription<DetectedObjects>(input_topics.at(i), rclcpp::QoS{1}, func);
   }
 
   is_initialized_ = true;
@@ -221,9 +221,7 @@ void InputManager::getObjectTimeInterval(
     object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_;
 }
 
-bool InputManager::getObjects(
-  const rclcpp::Time & now,
-  std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
+bool InputManager::getObjects(const rclcpp::Time & now, std::vector<DetectedObjects> & objects)
 {
   if (!is_initialized_) {
     RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized");
@@ -238,16 +236,14 @@ bool InputManager::getObjects(
   getObjectTimeInterval(now, object_latest_time, object_oldest_time);
 
   // Get objects from all input streams
+  // adds-up to the objects vector for efficient processing
   for (const auto & input_stream : input_streams_) {
     input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects);
   }
 
   // Sort objects by timestamp
   std::sort(
-    objects.begin(), objects.end(),
-    [](
-      const autoware_auto_perception_msgs::msg::DetectedObjects & a,
-      const autoware_auto_perception_msgs::msg::DetectedObjects & b) {
+    objects.begin(), objects.end(), [](const DetectedObjects & a, const DetectedObjects & b) {
       return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0;
     });
 
@@ -262,3 +258,5 @@ bool InputManager::getObjects(
 
   return !objects.empty();
 }
+
+}  // namespace multi_object_tracker

From 787d48bd343efc948a2cfb5ab821f19d16c3de23 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Tue, 16 Apr 2024 17:57:42 +0900
Subject: [PATCH 20/67] feat: object lists with detector index

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_core.hpp             |  2 +-
 .../processor/input_manager.hpp               |  6 +++--
 .../src/multi_object_tracker_core.cpp         | 27 ++++++++++---------
 .../src/processor/input_manager.cpp           | 26 ++++++++++--------
 4 files changed, 34 insertions(+), 27 deletions(-)

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
index 7ac0cbb303799..d917cb90e6fb6 100644
--- 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
@@ -95,7 +95,7 @@ class MultiObjectTracker : public rclcpp::Node
   // callback functions
   void onTimer();
   void onTrigger();
-  void onMessage(const std::vector<DetectedObjects> & objects_data);
+  void onMessage(const std::vector<std::pair<size_t, DetectedObjects>> & objects_data);
 
   // publish processes
   void runProcess(const DetectedObjects & input_objects_msg);
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index da19d5628be9c..116059426cd23 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -23,6 +23,7 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace multi_object_tracker
@@ -48,7 +49,7 @@ class InputStream
 
   void getObjectsOlderThan(
     const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
-    std::vector<DetectedObjects> & objects);
+    std::vector<std::pair<size_t, DetectedObjects>> & objects);
   void getNames(std::string & long_name, std::string & short_name)
   {
     long_name = long_name_;
@@ -105,7 +106,8 @@ class InputManager
 
   void getObjectTimeInterval(
     const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time);
-  bool getObjects(const rclcpp::Time & now, std::vector<DetectedObjects> & objects);
+  bool getObjects(
+    const rclcpp::Time & now, std::vector<std::pair<size_t, DetectedObjects>> & objects_list);
 
 private:
   rclcpp::Node & node_;
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index f3a272acdbad7..88ea287f32266 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -171,12 +171,12 @@ void MultiObjectTracker::onTrigger()
 {
   const rclcpp::Time current_time = this->now();
   // get objects from the input manager and run process
-  std::vector<DetectedObjects> objects_data;
-  const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data);
+  std::vector<std::pair<size_t, DetectedObjects>> objects_list;
+  const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list);
   if (!is_objects_ready) return;
 
-  onMessage(objects_data);
-  const rclcpp::Time latest_time(objects_data.back().header.stamp);
+  onMessage(objects_list);
+  const rclcpp::Time latest_time(objects_list.back().second.header.stamp);
 
   // Publish objects if the timer is not enabled
   if (publish_timer_ == nullptr) {
@@ -203,32 +203,33 @@ void MultiObjectTracker::onTimer()
   if (elapsed_time < maximum_publish_latency) return;
 
   // get objects from the input manager and run process
-  std::vector<DetectedObjects> objects_data;
-  const bool is_objects_ready = input_manager_->getObjects(current_time, objects_data);
+  std::vector<std::pair<size_t, DetectedObjects>> objects_list;
+  const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list);
   if (is_objects_ready) {
-    onMessage(objects_data);
+    onMessage(objects_list);
   }
 
   // Publish
   checkAndPublish(current_time);
 }
 
-void MultiObjectTracker::onMessage(const std::vector<DetectedObjects> & objects_data)
+void MultiObjectTracker::onMessage(
+  const std::vector<std::pair<size_t, DetectedObjects>> & objects_list)
 {
   const rclcpp::Time current_time = this->now();
+  const rclcpp::Time oldest_time(objects_list.front().second.header.stamp);
 
   // process start
-  debugger_->startMeasurementTime(this->now(), rclcpp::Time(objects_data.front().header.stamp));
+  debugger_->startMeasurementTime(this->now(), oldest_time);
   // run process for each DetectedObjects
-  for (const auto & objects : objects_data) {
-    runProcess(objects);
+  for (const auto & objects_data : objects_list) {
+    runProcess(objects_data.second);
   }
   // process end
   debugger_->endMeasurementTime(this->now());
 
   // for debug
-  const rclcpp::Time oldest_time(objects_data.front().header.stamp);
-  const rclcpp::Time latest_time(objects_data.back().header.stamp);
+  const rclcpp::Time latest_time(objects_list.back().second.header.stamp);
   RCLCPP_INFO(
     this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f",
     (current_time - latest_time).seconds(), (current_time - oldest_time).seconds());
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index b9ed035f1e2c6..bf2ee2165fb22 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -105,7 +105,7 @@ void InputStream::onMessage(
 
 void InputStream::getObjectsOlderThan(
   const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
-  std::vector<DetectedObjects> & objects)
+  std::vector<std::pair<size_t, DetectedObjects>> & objects_list)
 {
   assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds());
 
@@ -120,7 +120,8 @@ void InputStream::getObjectsOlderThan(
 
     // Add the object if the object is older than the specified latest time
     if (object_latest_time >= object_time) {
-      objects.push_back(object);
+      std::pair<size_t, DetectedObjects> object_pair(index_, object);
+      objects_list.push_back(object_pair);
       // remove the object from the queue
       objects_que_.pop_front();
     }
@@ -221,7 +222,8 @@ void InputManager::getObjectTimeInterval(
     object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_;
 }
 
-bool InputManager::getObjects(const rclcpp::Time & now, std::vector<DetectedObjects> & objects)
+bool InputManager::getObjects(
+  const rclcpp::Time & now, std::vector<std::pair<size_t, DetectedObjects>> & objects_list)
 {
   if (!is_initialized_) {
     RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized");
@@ -229,7 +231,7 @@ bool InputManager::getObjects(const rclcpp::Time & now, std::vector<DetectedObje
   }
 
   // Clear the objects
-  objects.clear();
+  objects_list.clear();
 
   // Get the time interval for the objects
   rclcpp::Time object_latest_time, object_oldest_time;
@@ -238,25 +240,27 @@ bool InputManager::getObjects(const rclcpp::Time & now, std::vector<DetectedObje
   // Get objects from all input streams
   // adds-up to the objects vector for efficient processing
   for (const auto & input_stream : input_streams_) {
-    input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects);
+    input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_list);
   }
 
   // Sort objects by timestamp
   std::sort(
-    objects.begin(), objects.end(), [](const DetectedObjects & a, const DetectedObjects & b) {
-      return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0;
+    objects_list.begin(), objects_list.end(),
+    [](const std::pair<size_t, DetectedObjects> & a, const std::pair<size_t, DetectedObjects> & b) {
+      return (rclcpp::Time(a.second.header.stamp) - rclcpp::Time(b.second.header.stamp)).seconds() <
+             0;
     });
 
   RCLCPP_INFO(
     node_.get_logger(), "InputManager::getObjects Got %zu objects from input streams",
-    objects.size());
+    objects_list.size());
 
   // Update the latest object time
-  if (!objects.empty()) {
-    latest_object_time_ = rclcpp::Time(objects.back().header.stamp);
+  if (!objects_list.empty()) {
+    latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp);
   }
 
-  return !objects.empty();
+  return !objects_list.empty();
 }
 
 }  // namespace multi_object_tracker

From b4993c2a1df2793b69b50a9145e1279427ce9594 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Tue, 16 Apr 2024 18:42:51 +0900
Subject: [PATCH 21/67] feat: define input channel struct

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_core.hpp             |  3 +-
 .../processor/input_manager.hpp               | 18 +++++---
 .../src/multi_object_tracker_core.cpp         | 43 ++++++++++++++-----
 .../src/processor/input_manager.cpp           | 29 ++++++-------
 4 files changed, 57 insertions(+), 36 deletions(-)

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
index d917cb90e6fb6..45ee40eddc369 100644
--- 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
@@ -88,9 +88,8 @@ class MultiObjectTracker : public rclcpp::Node
   // input manager
   std::unique_ptr<InputManager> input_manager_;
 
-  std::vector<std::string> input_topic_names_{};
+  std::vector<InputChannel> input_channels_{};
   size_t input_topic_size_{};
-  std::vector<std::pair<rclcpp::Time, DetectedObjects>> objects_data_{};
 
   // callback functions
   void onTimer();
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 116059426cd23..405b98ef1650a 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -28,16 +28,24 @@
 
 namespace multi_object_tracker
 {
-using autoware_auto_perception_msgs::msg::DetectedObject;
 using autoware_auto_perception_msgs::msg::DetectedObjects;
 
+struct InputChannel
+{
+  size_t index;             // index of the input channel
+  std::string input_topic;  // topic name of the detection, e.g. "/detection/lidar"
+  std::string long_name = "Detected Object";  // full name of the detection
+  std::string short_name = "DET";             // abbreviation of the name
+  double expected_rate = 10.0;                // [Hz]
+  double expected_latency = 0.2;              // [s]
+};
+
 class InputStream
 {
 public:
   explicit InputStream(rclcpp::Node & node, size_t & index);
 
-  void init(
-    const std::string & input_topic, const std::string & long_name, const std::string & short_name);
+  void init(const InputChannel & input_channel);
 
   void setQueueSize(size_t que_size) { que_size_ = que_size; }
   void setTriggerFunction(std::function<void(const size_t &)> func_trigger)
@@ -97,9 +105,7 @@ class InputManager
 public:
   explicit InputManager(rclcpp::Node & node);
 
-  void init(
-    const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names,
-    const std::vector<std::string> & short_names);
+  void init(const std::vector<InputChannel> & input_channels);
   void setTriggerFunction(std::function<void()> func_trigger) { func_trigger_ = func_trigger; }
 
   void onTrigger(const size_t & index) const;
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 88ea287f32266..e80cab8022ff9 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -87,6 +87,16 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   std::vector<std::string> input_names_long = get_parameter("input_names_long").as_string_array();
   std::vector<std::string> input_names_short = get_parameter("input_names_short").as_string_array();
 
+  input_channels_.resize(input_topic_names.size());
+  assert(input_topic_names.size() == input_names_long.size());
+  assert(input_topic_names.size() == input_names_short.size());
+  for (size_t i = 0; i < input_topic_names.size(); i++) {
+    input_channels_[i].index = i;
+    input_channels_[i].input_topic = input_topic_names[i];
+    input_channels_[i].long_name = input_names_long[i];
+    input_channels_[i].short_name = input_names_short[i];
+  }
+
   // ROS interface - Publisher
   tracked_objects_pub_ = create_publisher<TrackedObjects>("output", rclcpp::QoS{1});
 
@@ -97,18 +107,11 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   }
   input_topic_size_ = input_topic_names.size();
 
-  // print input information
-  for (size_t i = 0; i < input_topic_size_; i++) {
-    RCLCPP_INFO(
-      get_logger(), "Subscribing to %s %s %s", input_topic_names.at(i).c_str(),
-      input_names_long.at(i).c_str(), input_names_short.at(i).c_str());
-  }
-
   // Initialize input manager
   input_manager_ = std::make_unique<InputManager>(*this);
-  input_manager_->init(input_topic_names, input_names_long, input_names_short);
-  // Set trigger function
-  input_manager_->setTriggerFunction(std::bind(&MultiObjectTracker::onTrigger, this));
+  input_manager_->init(input_channels_);  // Initialize input manager, set subscriptions
+  input_manager_->setTriggerFunction(
+    std::bind(&MultiObjectTracker::onTrigger, this));  // Set trigger function
 
   // Create tf timer
   auto cti = std::make_shared<tf2_ros::CreateTimerROS>(
@@ -231,8 +234,26 @@ void MultiObjectTracker::onMessage(
   // for debug
   const rclcpp::Time latest_time(objects_list.back().second.header.stamp);
   RCLCPP_INFO(
-    this->get_logger(), "MultiObjectTracker::onTimer Objects time range: %f - %f",
+    this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f",
     (current_time - latest_time).seconds(), (current_time - oldest_time).seconds());
+
+  // count objects on each channel
+  std::vector<int> object_counts;
+  object_counts.resize(input_topic_size_);
+  // initialize to zero
+  for (size_t i = 0; i < input_topic_size_; i++) {
+    object_counts[i] = 0;
+  }
+  for (const auto & objects_data : objects_list) {
+    object_counts[objects_data.first] += 1;
+  }
+  // print result
+  std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: ";
+  for (size_t i = 0; i < input_topic_size_; i++) {
+    object_counts_str +=
+      input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + "  ";
+  }
+  RCLCPP_INFO(this->get_logger(), object_counts_str.c_str());
 }
 
 void MultiObjectTracker::runProcess(const DetectedObjects & input_objects_msg)
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index bf2ee2165fb22..605f4b98e9e1b 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -23,20 +23,19 @@ InputStream::InputStream(rclcpp::Node & node, size_t & index) : node_(node), ind
 {
 }
 
-void InputStream::init(
-  const std::string & input_topic, const std::string & long_name, const std::string & short_name)
+void InputStream::init(const InputChannel & input_channel)
 {
   // Initialize parameters
-  input_topic_ = input_topic;
-  long_name_ = long_name;
-  short_name_ = short_name;
+  input_topic_ = input_channel.input_topic;
+  long_name_ = input_channel.long_name;
+  short_name_ = input_channel.short_name;
 
   // Initialize queue
   objects_que_.clear();
 
   // Initialize latency statistics
-  expected_rate_ = 10.0;
-  latency_mean_ = 0.18;  // [s]
+  expected_rate_ = input_channel.expected_rate;    // [Hz]
+  latency_mean_ = input_channel.expected_latency;  // [s]
   latency_var_ = 0.0;
   interval_mean_ = 1 / expected_rate_;
   interval_var_ = 0.0;
@@ -136,24 +135,20 @@ InputManager::InputManager(rclcpp::Node & node) : node_(node)
   latest_object_time_ = node_.now();
 }
 
-void InputManager::init(
-  const std::vector<std::string> & input_topics, const std::vector<std::string> & long_names,
-  const std::vector<std::string> & short_names)
+void InputManager::init(const std::vector<InputChannel> & input_channels)
 {
   // Check input sizes
-  input_size_ = input_topics.size();
+  input_size_ = input_channels.size();
   if (input_size_ == 0) {
     RCLCPP_ERROR(node_.get_logger(), "InputManager::init No input streams");
     return;
   }
-  assert(input_size_ == long_names.size());
-  assert(input_size_ == short_names.size());
 
   sub_objects_array_.resize(input_size_);
 
   for (size_t i = 0; i < input_size_; i++) {
     InputStream input_stream(node_, i);
-    input_stream.init(input_topics.at(i), long_names.at(i), short_names.at(i));
+    input_stream.init(input_channels[i]);
     input_stream.setTriggerFunction(
       std::bind(&InputManager::onTrigger, this, std::placeholders::_1));
     input_streams_.push_back(std::make_shared<InputStream>(input_stream));
@@ -161,11 +156,11 @@ void InputManager::init(
     // Set subscription
     RCLCPP_INFO(
       node_.get_logger(), "InputManager::init Initializing %s input stream from %s",
-      long_names.at(i).c_str(), input_topics.at(i).c_str());
+      input_channels[i].long_name.c_str(), input_channels[i].input_topic.c_str());
     std::function<void(const DetectedObjects::ConstSharedPtr msg)> func =
       std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1);
-    sub_objects_array_.at(i) =
-      node_.create_subscription<DetectedObjects>(input_topics.at(i), rclcpp::QoS{1}, func);
+    sub_objects_array_.at(i) = node_.create_subscription<DetectedObjects>(
+      input_channels[i].input_topic, rclcpp::QoS{1}, func);
   }
 
   is_initialized_ = true;

From e495efbbfae2c05ee04481a8e46edb2e50a05106 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Wed, 17 Apr 2024 11:37:05 +0900
Subject: [PATCH 22/67] fix: define type for object list

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_core.hpp             |  8 +++---
 .../processor/input_manager.hpp               | 18 ++++++-------
 .../src/multi_object_tracker_core.cpp         | 26 +++++++++----------
 .../src/processor/input_manager.cpp           | 24 ++++++++---------
 4 files changed, 37 insertions(+), 39 deletions(-)

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
index 45ee40eddc369..956cf8406e508 100644
--- 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
@@ -55,9 +55,9 @@
 namespace multi_object_tracker
 {
 
-using autoware_auto_perception_msgs::msg::DetectedObject;
-using autoware_auto_perception_msgs::msg::DetectedObjects;
-using autoware_auto_perception_msgs::msg::TrackedObjects;
+using DetectedObject = autoware_auto_perception_msgs::msg::DetectedObject;
+using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects;
+using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects;
 
 class MultiObjectTracker : public rclcpp::Node
 {
@@ -94,7 +94,7 @@ class MultiObjectTracker : public rclcpp::Node
   // callback functions
   void onTimer();
   void onTrigger();
-  void onMessage(const std::vector<std::pair<size_t, DetectedObjects>> & objects_data);
+  void onMessage(const ObjectsList & objects_list);
 
   // publish processes
   void runProcess(const DetectedObjects & input_objects_msg);
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 405b98ef1650a..725503c880915 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -28,7 +28,8 @@
 
 namespace multi_object_tracker
 {
-using autoware_auto_perception_msgs::msg::DetectedObjects;
+using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects;
+using ObjectsList = std::vector<std::pair<uint, DetectedObjects>>;
 
 struct InputChannel
 {
@@ -43,12 +44,12 @@ struct InputChannel
 class InputStream
 {
 public:
-  explicit InputStream(rclcpp::Node & node, size_t & index);
+  explicit InputStream(rclcpp::Node & node, uint & index);
 
   void init(const InputChannel & input_channel);
 
   void setQueueSize(size_t que_size) { que_size_ = que_size; }
-  void setTriggerFunction(std::function<void(const size_t &)> func_trigger)
+  void setTriggerFunction(std::function<void(const uint &)> func_trigger)
   {
     func_trigger_ = func_trigger;
   }
@@ -57,7 +58,7 @@ class InputStream
 
   void getObjectsOlderThan(
     const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
-    std::vector<std::pair<size_t, DetectedObjects>> & objects);
+    ObjectsList & objects_list);
   void getNames(std::string & long_name, std::string & short_name)
   {
     long_name = long_name_;
@@ -78,7 +79,7 @@ class InputStream
 
 private:
   rclcpp::Node & node_;
-  size_t index_;
+  uint index_;
 
   std::string input_topic_;
   std::string long_name_;
@@ -87,7 +88,7 @@ class InputStream
   size_t que_size_{30};
   std::deque<DetectedObjects> objects_que_;
 
-  std::function<void(const size_t &)> func_trigger_;
+  std::function<void(const uint &)> func_trigger_;
 
   bool is_time_initialized_{false};
   double expected_rate_;
@@ -108,12 +109,11 @@ class InputManager
   void init(const std::vector<InputChannel> & input_channels);
   void setTriggerFunction(std::function<void()> func_trigger) { func_trigger_ = func_trigger; }
 
-  void onTrigger(const size_t & index) const;
+  void onTrigger(const uint & index) const;
 
   void getObjectTimeInterval(
     const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time);
-  bool getObjects(
-    const rclcpp::Time & now, std::vector<std::pair<size_t, DetectedObjects>> & objects_list);
+  bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list);
 
 private:
   rclcpp::Node & node_;
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index e80cab8022ff9..c89dd3d82a4f8 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -87,16 +87,6 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   std::vector<std::string> input_names_long = get_parameter("input_names_long").as_string_array();
   std::vector<std::string> input_names_short = get_parameter("input_names_short").as_string_array();
 
-  input_channels_.resize(input_topic_names.size());
-  assert(input_topic_names.size() == input_names_long.size());
-  assert(input_topic_names.size() == input_names_short.size());
-  for (size_t i = 0; i < input_topic_names.size(); i++) {
-    input_channels_[i].index = i;
-    input_channels_[i].input_topic = input_topic_names[i];
-    input_channels_[i].long_name = input_names_long[i];
-    input_channels_[i].short_name = input_names_short[i];
-  }
-
   // ROS interface - Publisher
   tracked_objects_pub_ = create_publisher<TrackedObjects>("output", rclcpp::QoS{1});
 
@@ -106,6 +96,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     return;
   }
   input_topic_size_ = input_topic_names.size();
+  input_channels_.resize(input_topic_size_);
+  assert(input_names_long.size() == input_topic_size_);
+  assert(input_names_short.size() == input_topic_size_);
+  for (size_t i = 0; i < input_topic_size_; i++) {
+    input_channels_[i].index = i;
+    input_channels_[i].input_topic = input_topic_names[i];
+    input_channels_[i].long_name = input_names_long[i];
+    input_channels_[i].short_name = input_names_short[i];
+  }
 
   // Initialize input manager
   input_manager_ = std::make_unique<InputManager>(*this);
@@ -174,7 +173,7 @@ void MultiObjectTracker::onTrigger()
 {
   const rclcpp::Time current_time = this->now();
   // get objects from the input manager and run process
-  std::vector<std::pair<size_t, DetectedObjects>> objects_list;
+  std::vector<std::pair<uint, DetectedObjects>> objects_list;
   const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list);
   if (!is_objects_ready) return;
 
@@ -206,7 +205,7 @@ void MultiObjectTracker::onTimer()
   if (elapsed_time < maximum_publish_latency) return;
 
   // get objects from the input manager and run process
-  std::vector<std::pair<size_t, DetectedObjects>> objects_list;
+  ObjectsList objects_list;
   const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list);
   if (is_objects_ready) {
     onMessage(objects_list);
@@ -216,8 +215,7 @@ void MultiObjectTracker::onTimer()
   checkAndPublish(current_time);
 }
 
-void MultiObjectTracker::onMessage(
-  const std::vector<std::pair<size_t, DetectedObjects>> & objects_list)
+void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
 {
   const rclcpp::Time current_time = this->now();
   const rclcpp::Time oldest_time(objects_list.front().second.header.stamp);
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 605f4b98e9e1b..5120f6632ead4 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -19,7 +19,7 @@
 namespace multi_object_tracker
 {
 
-InputStream::InputStream(rclcpp::Node & node, size_t & index) : node_(node), index_(index)
+InputStream::InputStream(rclcpp::Node & node, uint & index) : node_(node), index_(index)
 {
 }
 
@@ -104,7 +104,7 @@ void InputStream::onMessage(
 
 void InputStream::getObjectsOlderThan(
   const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
-  std::vector<std::pair<size_t, DetectedObjects>> & objects_list)
+  ObjectsList & objects_list)
 {
   assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds());
 
@@ -119,7 +119,7 @@ void InputStream::getObjectsOlderThan(
 
     // Add the object if the object is older than the specified latest time
     if (object_latest_time >= object_time) {
-      std::pair<size_t, DetectedObjects> object_pair(index_, object);
+      std::pair<uint, DetectedObjects> object_pair(index_, object);
       objects_list.push_back(object_pair);
       // remove the object from the queue
       objects_que_.pop_front();
@@ -147,7 +147,8 @@ void InputManager::init(const std::vector<InputChannel> & input_channels)
   sub_objects_array_.resize(input_size_);
 
   for (size_t i = 0; i < input_size_; i++) {
-    InputStream input_stream(node_, i);
+    uint index(i);
+    InputStream input_stream(node_, index);
     input_stream.init(input_channels[i]);
     input_stream.setTriggerFunction(
       std::bind(&InputManager::onTrigger, this, std::placeholders::_1));
@@ -166,10 +167,10 @@ void InputManager::init(const std::vector<InputChannel> & input_channels)
   is_initialized_ = true;
 }
 
-void InputManager::onTrigger(const size_t & index) const
+void InputManager::onTrigger(const uint & index) const
 {
   // input stream index of 0 is the target(main) input stream
-  const size_t target_idx = 0;
+  const uint target_idx = 0;
 
   // when the main stream triggers, call the trigger function
   if (index == target_idx && func_trigger_) {
@@ -203,11 +204,11 @@ void InputManager::getObjectTimeInterval(
   }
 
   // Get proper latency
-  constexpr double target_latency = 0.15;  // [s], measurement to tracking latency
+  constexpr double target_latency = 0.18;  // [s], measurement to tracking latency
                                            // process latency of a main detection + margin
-
   constexpr double acceptable_latency =
-    0.35;  // [s], acceptable band from the target latency, larger than the target latency
+    0.32;  // [s], acceptable band from the target latency, larger than the target latency
+
   object_latest_time = now - rclcpp::Duration::from_seconds(target_latency);
   object_oldest_time = now - rclcpp::Duration::from_seconds(acceptable_latency);
 
@@ -217,8 +218,7 @@ void InputManager::getObjectTimeInterval(
     object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_;
 }
 
-bool InputManager::getObjects(
-  const rclcpp::Time & now, std::vector<std::pair<size_t, DetectedObjects>> & objects_list)
+bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_list)
 {
   if (!is_initialized_) {
     RCLCPP_INFO(node_.get_logger(), "InputManager::getObjects Input manager is not initialized");
@@ -241,7 +241,7 @@ bool InputManager::getObjects(
   // Sort objects by timestamp
   std::sort(
     objects_list.begin(), objects_list.end(),
-    [](const std::pair<size_t, DetectedObjects> & a, const std::pair<size_t, DetectedObjects> & b) {
+    [](const std::pair<uint, DetectedObjects> & a, const std::pair<uint, DetectedObjects> & b) {
       return (rclcpp::Time(a.second.header.stamp) - rclcpp::Time(b.second.header.stamp)).seconds() <
              0;
     });

From 4955207c037ae966da7cd77a8f4ee6cd3e88a78e Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Wed, 17 Apr 2024 17:39:55 +0900
Subject: [PATCH 23/67] feat: add channel wise existence probability

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_core.hpp             |  4 +-
 .../processor/processor.hpp                   | 10 +--
 .../tracker/model/bicycle_tracker.hpp         |  3 +-
 .../tracker/model/big_vehicle_tracker.hpp     |  3 +-
 .../model/multiple_vehicle_tracker.hpp        |  3 +-
 .../tracker/model/normal_vehicle_tracker.hpp  |  3 +-
 .../tracker/model/pass_through_tracker.hpp    |  3 +-
 .../model/pedestrian_and_bicycle_tracker.hpp  |  3 +-
 .../tracker/model/pedestrian_tracker.hpp      |  3 +-
 .../tracker/model/tracker_base.hpp            | 48 ++++++++-----
 .../tracker/model/unknown_tracker.hpp         |  3 +-
 .../src/multi_object_tracker_core.cpp         | 27 ++++----
 .../src/processor/processor.cpp               | 43 +++++++-----
 .../src/tracker/model/bicycle_tracker.cpp     |  8 ++-
 .../src/tracker/model/big_vehicle_tracker.cpp |  8 ++-
 .../model/multiple_vehicle_tracker.cpp        | 11 +--
 .../tracker/model/normal_vehicle_tracker.cpp  |  8 ++-
 .../tracker/model/pass_through_tracker.cpp    |  8 ++-
 .../model/pedestrian_and_bicycle_tracker.cpp  | 11 +--
 .../src/tracker/model/pedestrian_tracker.cpp  |  8 ++-
 .../src/tracker/model/tracker_base.cpp        | 67 +++++++++++++++++--
 .../src/tracker/model/unknown_tracker.cpp     |  8 ++-
 22 files changed, 206 insertions(+), 87 deletions(-)

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
index 956cf8406e508..1df852cfefb02 100644
--- 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
@@ -89,7 +89,7 @@ class MultiObjectTracker : public rclcpp::Node
   std::unique_ptr<InputManager> input_manager_;
 
   std::vector<InputChannel> input_channels_{};
-  size_t input_topic_size_{};
+  size_t input_channel_size_{};
 
   // callback functions
   void onTimer();
@@ -97,7 +97,7 @@ class MultiObjectTracker : public rclcpp::Node
   void onMessage(const ObjectsList & objects_list);
 
   // publish processes
-  void runProcess(const DetectedObjects & input_objects_msg);
+  void runProcess(const DetectedObjects & input_objects_msg, const uint & channel_index);
   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
index 654c960463933..0b27072c19e4d 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp
@@ -32,7 +32,8 @@
 class TrackerProcessor
 {
 public:
-  explicit TrackerProcessor(const std::map<std::uint8_t, std::string> & tracker_map);
+  explicit TrackerProcessor(
+    const std::map<std::uint8_t, std::string> & tracker_map, const size_t & channel_size);
 
   const std::list<std::shared_ptr<Tracker>> & getListTracker() const { return list_tracker_; }
   // tracker processes
@@ -40,11 +41,11 @@ class TrackerProcessor
   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);
+    const std::unordered_map<int, int> & direct_assignment, const uint & channel_index);
   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);
+    const std::unordered_map<int, int> & reverse_assignment, const uint & channel_index);
   void prune(const rclcpp::Time & time);
 
   // output
@@ -59,6 +60,7 @@ class TrackerProcessor
 private:
   std::map<std::uint8_t, std::string> tracker_map_;
   std::list<std::shared_ptr<Tracker>> list_tracker_;
+  const size_t channel_size_;
 
   // parameters
   float max_elapsed_time_;            // [s]
@@ -71,7 +73,7 @@ class TrackerProcessor
   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;
+    const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const;
 };
 
 #endif  // MULTI_OBJECT_TRACKER__PROCESSOR__PROCESSOR_HPP_
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp
index 2d69334a2f43f..ead0d8dbf0e5a 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp
@@ -56,7 +56,8 @@ class BicycleTracker : public Tracker
 public:
   BicycleTracker(
     const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform);
+    const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+    const uint & channel_index);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
index 8d49138b94a24..3b509a3cb47d8 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
@@ -61,7 +61,8 @@ class BigVehicleTracker : public Tracker
 public:
   BigVehicleTracker(
     const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform);
+    const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+    const uint & channel_index);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp
index 5722566b4a633..fef8caaf20d8a 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp
@@ -35,7 +35,8 @@ class MultipleVehicleTracker : public Tracker
 public:
   MultipleVehicleTracker(
     const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform);
+    const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+    const uint & channel_index);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
index 6ab7e63bce167..b7810d2471416 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
@@ -61,7 +61,8 @@ class NormalVehicleTracker : public Tracker
 public:
   NormalVehicleTracker(
     const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform);
+    const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+    const uint & channel_index);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp
index 48a8702d182a0..0f892098a373a 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pass_through_tracker.hpp
@@ -34,7 +34,8 @@ class PassThroughTracker : public Tracker
 public:
   PassThroughTracker(
     const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform);
+    const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+    const uint & channel_index);
   bool predict(const rclcpp::Time & time) override;
   bool measure(
     const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp
index cf6a82088c7b6..490f6d93c6f6a 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp
@@ -34,7 +34,8 @@ class PedestrianAndBicycleTracker : public Tracker
 public:
   PedestrianAndBicycleTracker(
     const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform);
+    const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+    const uint & channel_index);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp
index a56250390e34f..72f5a5a002f0f 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp
@@ -64,7 +64,8 @@ class PedestrianTracker : public Tracker
 public:
   PedestrianTracker(
     const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform);
+    const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+    const uint & channel_index);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp
index ab6a747288d4a..b25e1655b5fdb 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp
@@ -35,33 +35,36 @@
 
 class Tracker
 {
-protected:
-  unique_identifier_msgs::msg::UUID getUUID() const { return uuid_; }
-  void setClassification(
-    const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification)
-  {
-    classification_ = classification;
-  }
-  void updateClassification(
-    const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification);
-
 private:
   unique_identifier_msgs::msg::UUID uuid_;
+
+  // classification
   std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> classification_;
+
+  // existence states
   int no_measurement_count_;
   int total_no_measurement_count_;
   int total_measurement_count_;
   rclcpp::Time last_update_with_measurement_time_;
+  std::vector<float> existence_probabilities_;
+  float total_existence_probability_;
 
 public:
   Tracker(
     const rclcpp::Time & time,
-    const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification);
+    const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification,
+    const size_t & channel_size);
   virtual ~Tracker() {}
+
+  void initializeExistenceProbabilities(
+    const uint & channel_index, const float & existence_probability);
   bool updateWithMeasurement(
     const autoware_auto_perception_msgs::msg::DetectedObject & object,
-    const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform);
-  bool updateWithoutMeasurement();
+    const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform,
+    const uint & channel_index);
+  bool updateWithoutMeasurement(const rclcpp::Time & now);
+
+  // classification
   std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> getClassification() const
   {
     return classification_;
@@ -70,6 +73,8 @@ class Tracker
   {
     return object_recognition_utils::getHighestProbLabel(classification_);
   }
+
+  // existence states
   int getNoMeasurementCount() const { return no_measurement_count_; }
   int getTotalNoMeasurementCount() const { return total_no_measurement_count_; }
   int getTotalMeasurementCount() const { return total_measurement_count_; }
@@ -77,13 +82,22 @@ class Tracker
   {
     return (current_time - last_update_with_measurement_time_).seconds();
   }
+
+protected:
+  unique_identifier_msgs::msg::UUID getUUID() const { return uuid_; }
+  void setClassification(
+    const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification)
+  {
+    classification_ = classification;
+  }
+  void updateClassification(
+    const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification);
+
+  // virtual functions
+public:
   virtual geometry_msgs::msg::PoseWithCovariance getPoseWithCovariance(
     const rclcpp::Time & time) const;
 
-  /*
-   * Pure virtual function
-   */
-
 protected:
   virtual bool measure(
     const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp
index 73bf7849e13d8..3ef58773b408f 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/unknown_tracker.hpp
@@ -49,7 +49,8 @@ class UnknownTracker : public Tracker
 public:
   UnknownTracker(
     const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-    const geometry_msgs::msg::Transform & self_transform);
+    const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+    const uint & channel_index);
 
   bool predict(const rclcpp::Time & time) override;
   bool measure(
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index c89dd3d82a4f8..74f3a24ee0347 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -95,11 +95,11 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     RCLCPP_ERROR(get_logger(), "Need a 'input_topic_names' parameter to be set before continuing");
     return;
   }
-  input_topic_size_ = input_topic_names.size();
-  input_channels_.resize(input_topic_size_);
-  assert(input_names_long.size() == input_topic_size_);
-  assert(input_names_short.size() == input_topic_size_);
-  for (size_t i = 0; i < input_topic_size_; i++) {
+  input_channel_size_ = input_topic_names.size();
+  input_channels_.resize(input_channel_size_);
+  assert(input_names_long.size() == input_channel_size_);
+  assert(input_names_short.size() == input_channel_size_);
+  for (size_t i = 0; i < input_channel_size_; i++) {
     input_channels_[i].index = i;
     input_channels_[i].input_topic = input_topic_names[i];
     input_channels_[i].long_name = input_names_long[i];
@@ -146,7 +146,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     tracker_map.insert(std::make_pair(
       Label::MOTORCYCLE, this->declare_parameter<std::string>("motorcycle_tracker")));
 
-    processor_ = std::make_unique<TrackerProcessor>(tracker_map);
+    processor_ = std::make_unique<TrackerProcessor>(tracker_map, input_channel_size_);
   }
 
   // Data association initialization
@@ -224,7 +224,7 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
   debugger_->startMeasurementTime(this->now(), oldest_time);
   // run process for each DetectedObjects
   for (const auto & objects_data : objects_list) {
-    runProcess(objects_data.second);
+    runProcess(objects_data.second, objects_data.first);
   }
   // process end
   debugger_->endMeasurementTime(this->now());
@@ -237,9 +237,9 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
 
   // count objects on each channel
   std::vector<int> object_counts;
-  object_counts.resize(input_topic_size_);
+  object_counts.resize(input_channel_size_);
   // initialize to zero
-  for (size_t i = 0; i < input_topic_size_; i++) {
+  for (size_t i = 0; i < input_channel_size_; i++) {
     object_counts[i] = 0;
   }
   for (const auto & objects_data : objects_list) {
@@ -247,14 +247,15 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
   }
   // print result
   std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: ";
-  for (size_t i = 0; i < input_topic_size_; i++) {
+  for (size_t i = 0; i < input_channel_size_; i++) {
     object_counts_str +=
       input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + "  ";
   }
   RCLCPP_INFO(this->get_logger(), object_counts_str.c_str());
 }
 
-void MultiObjectTracker::runProcess(const DetectedObjects & input_objects_msg)
+void MultiObjectTracker::runProcess(
+  const DetectedObjects & input_objects_msg, const uint & channel_index)
 {
   // Get the time of the measurement
   const rclcpp::Time measurement_time =
@@ -289,13 +290,13 @@ void MultiObjectTracker::runProcess(const DetectedObjects & input_objects_msg)
   }
 
   /* tracker update */
-  processor_->update(transformed_objects, *self_transform, direct_assignment);
+  processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index);
 
   /* tracker pruning */
   processor_->prune(measurement_time);
 
   /* spawn new tracker */
-  processor_->spawn(transformed_objects, *self_transform, reverse_assignment);
+  processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index);
 }
 
 void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time)
diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp
index 8cfb64cab19be..a5a1f18a64ca0 100644
--- a/perception/multi_object_tracker/src/processor/processor.cpp
+++ b/perception/multi_object_tracker/src/processor/processor.cpp
@@ -23,8 +23,9 @@
 
 using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
-TrackerProcessor::TrackerProcessor(const std::map<std::uint8_t, std::string> & tracker_map)
-: tracker_map_(tracker_map)
+TrackerProcessor::TrackerProcessor(
+  const std::map<std::uint8_t, std::string> & tracker_map, const size_t & channel_size)
+: tracker_map_(tracker_map), channel_size_(channel_size)
 {
   // Set tracker lifetime parameters
   max_elapsed_time_ = 1.0;  // [s]
@@ -48,7 +49,7 @@ void TrackerProcessor::predict(const rclcpp::Time & time)
 void TrackerProcessor::update(
   const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects,
   const geometry_msgs::msg::Transform & self_transform,
-  const std::unordered_map<int, int> & direct_assignment)
+  const std::unordered_map<int, int> & direct_assignment, const uint & channel_index)
 {
   int tracker_idx = 0;
   const auto & time = detected_objects.header.stamp;
@@ -57,9 +58,10 @@ void TrackerProcessor::update(
     if (direct_assignment.find(tracker_idx) != direct_assignment.end()) {  // found
       const auto & associated_object =
         detected_objects.objects.at(direct_assignment.find(tracker_idx)->second);
-      (*(tracker_itr))->updateWithMeasurement(associated_object, time, self_transform);
+      (*(tracker_itr))
+        ->updateWithMeasurement(associated_object, time, self_transform, channel_index);
     } else {  // not found
-      (*(tracker_itr))->updateWithoutMeasurement();
+      (*(tracker_itr))->updateWithoutMeasurement(time);
     }
   }
 }
@@ -67,7 +69,7 @@ void TrackerProcessor::update(
 void TrackerProcessor::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)
+  const std::unordered_map<int, int> & reverse_assignment, const uint & channel_index)
 {
   const auto & time = detected_objects.header.stamp;
   for (size_t i = 0; i < detected_objects.objects.size(); ++i) {
@@ -75,34 +77,43 @@ void TrackerProcessor::spawn(
       continue;
     }
     const auto & new_object = detected_objects.objects.at(i);
-    std::shared_ptr<Tracker> tracker = createNewTracker(new_object, time, self_transform);
+    std::shared_ptr<Tracker> tracker =
+      createNewTracker(new_object, time, self_transform, channel_index);
     if (tracker) list_tracker_.push_back(tracker);
   }
 }
 
 std::shared_ptr<Tracker> TrackerProcessor::createNewTracker(
   const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
-  const geometry_msgs::msg::Transform & self_transform) const
+  const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const
 {
   const std::uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification);
   if (tracker_map_.count(label) != 0) {
     const auto tracker = tracker_map_.at(label);
     if (tracker == "bicycle_tracker")
-      return std::make_shared<BicycleTracker>(time, object, self_transform);
+      return std::make_shared<BicycleTracker>(
+        time, object, self_transform, channel_size_, channel_index);
     if (tracker == "big_vehicle_tracker")
-      return std::make_shared<BigVehicleTracker>(time, object, self_transform);
+      return std::make_shared<BigVehicleTracker>(
+        time, object, self_transform, channel_size_, channel_index);
     if (tracker == "multi_vehicle_tracker")
-      return std::make_shared<MultipleVehicleTracker>(time, object, self_transform);
+      return std::make_shared<MultipleVehicleTracker>(
+        time, object, self_transform, channel_size_, channel_index);
     if (tracker == "normal_vehicle_tracker")
-      return std::make_shared<NormalVehicleTracker>(time, object, self_transform);
+      return std::make_shared<NormalVehicleTracker>(
+        time, object, self_transform, channel_size_, channel_index);
     if (tracker == "pass_through_tracker")
-      return std::make_shared<PassThroughTracker>(time, object, self_transform);
+      return std::make_shared<PassThroughTracker>(
+        time, object, self_transform, channel_size_, channel_index);
     if (tracker == "pedestrian_and_bicycle_tracker")
-      return std::make_shared<PedestrianAndBicycleTracker>(time, object, self_transform);
+      return std::make_shared<PedestrianAndBicycleTracker>(
+        time, object, self_transform, channel_size_, channel_index);
     if (tracker == "pedestrian_tracker")
-      return std::make_shared<PedestrianTracker>(time, object, self_transform);
+      return std::make_shared<PedestrianTracker>(
+        time, object, self_transform, channel_size_, channel_index);
   }
-  return std::make_shared<UnknownTracker>(time, object, self_transform);
+  return std::make_shared<UnknownTracker>(
+    time, object, self_transform, channel_size_, channel_index);
 }
 
 void TrackerProcessor::prune(const rclcpp::Time & time)
diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp
index 12c340516c6c1..0698e4651b6bf 100644
--- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp
+++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp
@@ -44,13 +44,17 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
 BicycleTracker::BicycleTracker(
   const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-  const geometry_msgs::msg::Transform & /*self_transform*/)
-: Tracker(time, object.classification),
+  const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size,
+  const uint & channel_index)
+: Tracker(time, object.classification, channel_size),
   logger_(rclcpp::get_logger("BicycleTracker")),
   z_(object.kinematics.pose_with_covariance.pose.position.z)
 {
   object_ = object;
 
+  // initialize existence probability
+  initializeExistenceProbabilities(channel_index, object.existence_probability);
+
   // Initialize parameters
   // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty
   double r_stddev_x = 0.5;                                  // in vehicle coordinate [m]
diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
index 3c73b9f19cfbb..e8ca8a47bbc78 100644
--- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
+++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
@@ -44,14 +44,18 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
 BigVehicleTracker::BigVehicleTracker(
   const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-  const geometry_msgs::msg::Transform & self_transform)
-: Tracker(time, object.classification),
+  const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+  const uint & channel_index)
+: Tracker(time, object.classification, channel_size),
   logger_(rclcpp::get_logger("BigVehicleTracker")),
   z_(object.kinematics.pose_with_covariance.pose.position.z),
   tracking_offset_(Eigen::Vector2d::Zero())
 {
   object_ = object;
 
+  // initialize existence probability
+  initializeExistenceProbabilities(channel_index, object.existence_probability);
+
   // Initialize parameters
   // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty
   float r_stddev_x = 0.5;                                  // in vehicle coordinate [m]
diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp
index 4f0fb4d7871f2..c925976f65e7e 100644
--- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp
+++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp
@@ -22,11 +22,14 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
 MultipleVehicleTracker::MultipleVehicleTracker(
   const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-  const geometry_msgs::msg::Transform & self_transform)
-: Tracker(time, object.classification),
-  normal_vehicle_tracker_(time, object, self_transform),
-  big_vehicle_tracker_(time, object, self_transform)
+  const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+  const uint & channel_index)
+: Tracker(time, object.classification, channel_size),
+  normal_vehicle_tracker_(time, object, self_transform, channel_size, channel_index),
+  big_vehicle_tracker_(time, object, self_transform, channel_size, channel_index)
 {
+  // initialize existence probability
+  initializeExistenceProbabilities(channel_index, object.existence_probability);
 }
 
 bool MultipleVehicleTracker::predict(const rclcpp::Time & time)
diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp
index 19fc5a2f71122..54d6e84dd6656 100644
--- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp
+++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp
@@ -44,14 +44,18 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
 NormalVehicleTracker::NormalVehicleTracker(
   const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-  const geometry_msgs::msg::Transform & self_transform)
-: Tracker(time, object.classification),
+  const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+  const uint & channel_index)
+: Tracker(time, object.classification, channel_size),
   logger_(rclcpp::get_logger("NormalVehicleTracker")),
   z_(object.kinematics.pose_with_covariance.pose.position.z),
   tracking_offset_(Eigen::Vector2d::Zero())
 {
   object_ = object;
 
+  // initialize existence probability
+  initializeExistenceProbabilities(channel_index, object.existence_probability);
+
   // Initialize parameters
   // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty
   float r_stddev_x = 0.5;                                  // in vehicle coordinate [m]
diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp
index 2084ac28e70f0..e1f4383cf11a3 100644
--- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp
+++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp
@@ -37,13 +37,17 @@
 
 PassThroughTracker::PassThroughTracker(
   const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-  const geometry_msgs::msg::Transform & /*self_transform*/)
-: Tracker(time, object.classification),
+  const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size,
+  const uint & channel_index)
+: Tracker(time, object.classification, channel_size),
   logger_(rclcpp::get_logger("PassThroughTracker")),
   last_update_time_(time)
 {
   object_ = object;
   prev_observed_object_ = object;
+
+  // initialize existence probability
+  initializeExistenceProbabilities(channel_index, object.existence_probability);
 }
 
 bool PassThroughTracker::predict(const rclcpp::Time & time)
diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp
index d61a9a02ccd80..e399dade880fa 100644
--- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp
+++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp
@@ -22,11 +22,14 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
 PedestrianAndBicycleTracker::PedestrianAndBicycleTracker(
   const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-  const geometry_msgs::msg::Transform & self_transform)
-: Tracker(time, object.classification),
-  pedestrian_tracker_(time, object, self_transform),
-  bicycle_tracker_(time, object, self_transform)
+  const geometry_msgs::msg::Transform & self_transform, const size_t channel_size,
+  const uint & channel_index)
+: Tracker(time, object.classification, channel_size),
+  pedestrian_tracker_(time, object, self_transform, channel_size, channel_index),
+  bicycle_tracker_(time, object, self_transform, channel_size, channel_index)
 {
+  // initialize existence probability
+  initializeExistenceProbabilities(channel_index, object.existence_probability);
 }
 
 bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time)
diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp
index e1c07388836f6..0d4411b5266ad 100644
--- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp
+++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp
@@ -44,13 +44,17 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 
 PedestrianTracker::PedestrianTracker(
   const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-  const geometry_msgs::msg::Transform & /*self_transform*/)
-: Tracker(time, object.classification),
+  const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size,
+  const uint & channel_index)
+: Tracker(time, object.classification, channel_size),
   logger_(rclcpp::get_logger("PedestrianTracker")),
   z_(object.kinematics.pose_with_covariance.pose.position.z)
 {
   object_ = object;
 
+  // initialize existence probability
+  initializeExistenceProbabilities(channel_index, object.existence_probability);
+
   // Initialize parameters
   // measurement noise covariance
   float r_stddev_x = 0.4;                                  // [m]
diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
index a3320ff54afcb..4d2bb3301fe38 100644
--- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
+++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
@@ -23,7 +23,8 @@
 
 Tracker::Tracker(
   const rclcpp::Time & time,
-  const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification)
+  const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification,
+  const size_t & channel_size)
 : classification_(classification),
   no_measurement_count_(0),
   total_no_measurement_count_(0),
@@ -34,23 +35,75 @@ Tracker::Tracker(
   std::mt19937 gen(std::random_device{}());
   std::independent_bits_engine<std::mt19937, 8, uint8_t> bit_eng(gen);
   std::generate(uuid_.uuid.begin(), uuid_.uuid.end(), bit_eng);
+
+  // Initialize existence probabilities
+  existence_probabilities_.resize(channel_size, 0.0);
+}
+
+void Tracker::initializeExistenceProbabilities(
+  const uint & channel_index, const float & existence_probability)
+{
+  existence_probabilities_[channel_index] = existence_probability;
+  total_existence_probability_ = existence_probability;
 }
 
 bool Tracker::updateWithMeasurement(
   const autoware_auto_perception_msgs::msg::DetectedObject & object,
-  const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform)
+  const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform,
+  const uint & channel_index)
 {
-  no_measurement_count_ = 0;
-  ++total_measurement_count_;
+  // Update existence probability
+  {
+    float existence_probability_from_object = object.existence_probability;
+    no_measurement_count_ = 0;
+    ++total_measurement_count_;
+
+    double const delta_time = (measurement_time - last_update_with_measurement_time_).seconds();
+    double const decay_rate = 5.0 / 10.0;
+    existence_probabilities_[channel_index] = existence_probability_from_object;
+    for (size_t i = 0; i < existence_probabilities_.size(); ++i) {
+      if (i == channel_index) {
+        continue;
+      }
+      existence_probabilities_[i] *= std::exp(-decay_rate * delta_time);
+    }
+
+    // regularization
+    total_existence_probability_ =
+      std::accumulate(existence_probabilities_.begin(), existence_probabilities_.end(), 0.0f);
+    total_existence_probability_ = std::max(total_existence_probability_, 0.0f);
+    total_existence_probability_ = std::min(total_existence_probability_, 1.0f);
+  }
+
   last_update_with_measurement_time_ = measurement_time;
+
+  // Update object
   measure(object, measurement_time, self_transform);
+
   return true;
 }
 
-bool Tracker::updateWithoutMeasurement()
+bool Tracker::updateWithoutMeasurement(const rclcpp::Time & now)
 {
-  ++no_measurement_count_;
-  ++total_no_measurement_count_;
+  // Update existence probability
+  {
+    ++no_measurement_count_;
+    ++total_no_measurement_count_;
+
+    // decay
+    double const delta_time = (now - last_update_with_measurement_time_).seconds();
+    double const decay_rate = 5.0 / 10.0;
+    for (size_t i = 0; i < existence_probabilities_.size(); ++i) {
+      existence_probabilities_[i] *= std::exp(-decay_rate * delta_time);
+    }
+
+    // regularization
+    total_existence_probability_ =
+      std::accumulate(existence_probabilities_.begin(), existence_probabilities_.end(), 0.0f);
+    total_existence_probability_ = std::max(total_existence_probability_, 0.0f);
+    total_existence_probability_ = std::min(total_existence_probability_, 1.0f);
+  }
+
   return true;
 }
 
diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp
index 450bb2d94abb6..04638267f7ad8 100644
--- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp
+++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp
@@ -38,13 +38,17 @@
 
 UnknownTracker::UnknownTracker(
   const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object,
-  const geometry_msgs::msg::Transform & /*self_transform*/)
-: Tracker(time, object.classification),
+  const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size,
+  const uint & channel_index)
+: Tracker(time, object.classification, channel_size),
   logger_(rclcpp::get_logger("UnknownTracker")),
   z_(object.kinematics.pose_with_covariance.pose.position.z)
 {
   object_ = object;
 
+  // initialize existence probability
+  initializeExistenceProbabilities(channel_index, object.existence_probability);
+
   // initialize params
   // measurement noise covariance
   constexpr double r_stddev_x = 1.0;  // [m]

From 46c16c7afd75b44228973dbc45ee742d01173f5f Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 18 Apr 2024 11:35:37 +0900
Subject: [PATCH 24/67] fix: relocate debugger

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker/CMakeLists.txt       |  2 +-
 .../{ => debugger}/debugger.hpp               | 44 +++++++++++--------
 .../multi_object_tracker_core.hpp             |  2 +-
 .../src/{ => debugger}/debugger.cpp           | 40 +++++++++--------
 4 files changed, 50 insertions(+), 38 deletions(-)
 rename perception/multi_object_tracker/include/multi_object_tracker/{ => debugger}/debugger.hpp (93%)
 rename perception/multi_object_tracker/src/{ => debugger}/debugger.cpp (96%)

diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt
index 7d779d3bfc093..6adc0606add56 100644
--- a/perception/multi_object_tracker/CMakeLists.txt
+++ b/perception/multi_object_tracker/CMakeLists.txt
@@ -22,7 +22,7 @@ include_directories(
 # Generate exe file
 set(MULTI_OBJECT_TRACKER_SRC
   src/multi_object_tracker_core.cpp
-  src/debugger.cpp
+  src/debugger/debugger.cpp
   src/processor/processor.cpp
   src/processor/input_manager.cpp
   src/data_association/data_association.cpp
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
similarity index 93%
rename from perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp
rename to perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
index 90291ae6fec18..2c10fabd2a91e 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
@@ -14,8 +14,8 @@
 //
 //
 
-#ifndef MULTI_OBJECT_TRACKER__DEBUGGER_HPP_
-#define MULTI_OBJECT_TRACKER__DEBUGGER_HPP_
+#ifndef MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_
+#define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_
 
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <diagnostic_updater/publisher.hpp>
@@ -37,16 +37,8 @@ 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);
+private:
   struct DEBUG_SETTINGS
   {
     bool publish_processing_time;
@@ -54,22 +46,38 @@ class TrackerDebugger
     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_;
+
+  diagnostic_updater::Updater diagnostic_updater_;
+
+  bool is_initialized_ = false;
+  double pipeline_latency_ms_ = 0.0;
   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_;
+
+public:
+  bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; }
+  void setupDiagnostics();
+
+  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 checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat);
+
+private:
+  void loadParameters();
 };
 
-#endif  // MULTI_OBJECT_TRACKER__DEBUGGER_HPP_
+#endif  // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_
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
index 1df852cfefb02..3daeaa2a46322 100644
--- 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
@@ -20,7 +20,7 @@
 #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/debugger/debugger.hpp"
 #include "multi_object_tracker/processor/input_manager.hpp"
 #include "multi_object_tracker/processor/processor.hpp"
 #include "multi_object_tracker/tracker/model/tracker_base.hpp"
diff --git a/perception/multi_object_tracker/src/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp
similarity index 96%
rename from perception/multi_object_tracker/src/debugger.cpp
rename to perception/multi_object_tracker/src/debugger/debugger.cpp
index b5632a13e78fc..9b4906967d3af 100644
--- a/perception/multi_object_tracker/src/debugger.cpp
+++ b/perception/multi_object_tracker/src/debugger/debugger.cpp
@@ -14,11 +14,11 @@
 //
 //
 
-#include "multi_object_tracker/debugger.hpp"
+#include "multi_object_tracker/debugger/debugger.hpp"
 
 #include <memory>
 
-TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&node), node_(node)
+TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : node_(node), diagnostic_updater_(&node)
 {
   // declare debug parameters to decide whether to publish debug topics
   loadParameters();
@@ -46,14 +46,6 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&nod
   setupDiagnostics();
 }
 
-void TrackerDebugger::setupDiagnostics()
-{
-  diagnostic_updater_.setHardwareID(node_.get_name());
-  diagnostic_updater_.add(
-    "Perception delay check from original header stamp", this, &TrackerDebugger::checkDelay);
-  diagnostic_updater_.setPeriod(0.1);
-}
-
 void TrackerDebugger::loadParameters()
 {
   try {
@@ -74,6 +66,26 @@ void TrackerDebugger::loadParameters()
   }
 }
 
+void TrackerDebugger::setupDiagnostics()
+{
+  diagnostic_updater_.setHardwareID(node_.get_name());
+  diagnostic_updater_.add(
+    "Perception delay check from original header stamp", this, &TrackerDebugger::checkDelay);
+  diagnostic_updater_.setPeriod(0.1);
+}
+
+// object publishing functions
+
+void TrackerDebugger::publishTentativeObjects(
+  const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const
+{
+  if (debug_settings_.publish_tentative_objects) {
+    debug_tentative_objects_pub_->publish(tentative_objects);
+  }
+}
+
+// time measurement functions
+
 void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat)
 {
   if (!is_initialized_) {
@@ -97,14 +109,6 @@ void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & s
   stat.add("Detection delay", delay);
 }
 
-void TrackerDebugger::publishTentativeObjects(
-  const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const
-{
-  if (debug_settings_.publish_tentative_objects) {
-    debug_tentative_objects_pub_->publish(tentative_objects);
-  }
-}
-
 void TrackerDebugger::startMeasurementTime(
   const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp)
 {

From 99ad038dec2b1c1196996d6a6e92db8bc3607328 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 18 Apr 2024 14:26:25 +0900
Subject: [PATCH 25/67] fix: total existence logic change

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../processor/processor.hpp                   |  2 ++
 .../tracker/model/tracker_base.hpp            |  5 ++++
 .../src/debugger/debugger.cpp                 |  6 ++---
 .../src/tracker/model/tracker_base.cpp        | 27 +++++++------------
 4 files changed, 19 insertions(+), 21 deletions(-)

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
index 0b27072c19e4d..de0fe0854254a 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp
@@ -57,6 +57,8 @@ class TrackerProcessor
     const rclcpp::Time & time,
     autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const;
 
+  void getExistenceProbabilities(std::vector<std::vector<float>> & existence_vectors) const;
+
 private:
   std::map<std::uint8_t, std::string> tracker_map_;
   std::list<std::shared_ptr<Tracker>> list_tracker_;
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp
index b25e1655b5fdb..348ba1f5d7383 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp
@@ -58,6 +58,11 @@ class Tracker
 
   void initializeExistenceProbabilities(
     const uint & channel_index, const float & existence_probability);
+  bool getExistenceProbabilityVector(std::vector<float> & existence_vector) const
+  {
+    existence_vector = existence_probabilities_;
+    return existence_vector.size() > 0;
+  }
   bool updateWithMeasurement(
     const autoware_auto_perception_msgs::msg::DetectedObject & object,
     const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform,
diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp
index 9b4906967d3af..30838220a566e 100644
--- a/perception/multi_object_tracker/src/debugger/debugger.cpp
+++ b/perception/multi_object_tracker/src/debugger/debugger.cpp
@@ -11,8 +11,6 @@
 // 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.
-//
-//
 
 #include "multi_object_tracker/debugger/debugger.hpp"
 
@@ -74,7 +72,7 @@ void TrackerDebugger::setupDiagnostics()
   diagnostic_updater_.setPeriod(0.1);
 }
 
-// object publishing functions
+// Object publishing functions
 
 void TrackerDebugger::publishTentativeObjects(
   const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const
@@ -84,7 +82,7 @@ void TrackerDebugger::publishTentativeObjects(
   }
 }
 
-// time measurement functions
+// Time measurement functions
 
 void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat)
 {
diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
index 4d2bb3301fe38..74912655ea0d6 100644
--- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
+++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
@@ -58,8 +58,9 @@ bool Tracker::updateWithMeasurement(
     no_measurement_count_ = 0;
     ++total_measurement_count_;
 
-    double const delta_time = (measurement_time - last_update_with_measurement_time_).seconds();
-    double const decay_rate = 5.0 / 10.0;
+    // existence probability on each channel
+    const double delta_time = (measurement_time - last_update_with_measurement_time_).seconds();
+    const double decay_rate = 5.0 / 10.0;
     existence_probabilities_[channel_index] = existence_probability_from_object;
     for (size_t i = 0; i < existence_probabilities_.size(); ++i) {
       if (i == channel_index) {
@@ -68,11 +69,9 @@ bool Tracker::updateWithMeasurement(
       existence_probabilities_[i] *= std::exp(-decay_rate * delta_time);
     }
 
-    // regularization
-    total_existence_probability_ =
-      std::accumulate(existence_probabilities_.begin(), existence_probabilities_.end(), 0.0f);
-    total_existence_probability_ = std::max(total_existence_probability_, 0.0f);
-    total_existence_probability_ = std::min(total_existence_probability_, 1.0f);
+    // total existence probability - object is detected
+    total_existence_probability_ +=
+      (1 - total_existence_probability_) * existence_probability_from_object;
   }
 
   last_update_with_measurement_time_ = measurement_time;
@@ -86,22 +85,16 @@ bool Tracker::updateWithMeasurement(
 bool Tracker::updateWithoutMeasurement(const rclcpp::Time & now)
 {
   // Update existence probability
+  ++no_measurement_count_;
+  ++total_no_measurement_count_;
   {
-    ++no_measurement_count_;
-    ++total_no_measurement_count_;
-
-    // decay
+    // decay existence probability
     double const delta_time = (now - last_update_with_measurement_time_).seconds();
     double const decay_rate = 5.0 / 10.0;
     for (size_t i = 0; i < existence_probabilities_.size(); ++i) {
       existence_probabilities_[i] *= std::exp(-decay_rate * delta_time);
     }
-
-    // regularization
-    total_existence_probability_ =
-      std::accumulate(existence_probabilities_.begin(), existence_probabilities_.end(), 0.0f);
-    total_existence_probability_ = std::max(total_existence_probability_, 0.0f);
-    total_existence_probability_ = std::min(total_existence_probability_, 1.0f);
+    total_existence_probability_ *= std::exp(-decay_rate * delta_time);
   }
 
   return true;

From 1a761f9ed48a4ec42b1a8917ba0b46b7d23eba7f Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 18 Apr 2024 17:12:06 +0900
Subject: [PATCH 26/67] feat: publishing object debug info, need to fix marker
 id

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker/CMakeLists.txt       |   1 +
 .../debugger/debug_object.hpp                 |  59 ++++++
 .../debugger/debugger.hpp                     |  32 ++-
 .../src/debugger/debug_object.cpp             | 200 ++++++++++++++++++
 .../src/debugger/debugger.cpp                 |  26 ++-
 .../src/multi_object_tracker_core.cpp         |  12 +-
 6 files changed, 320 insertions(+), 10 deletions(-)
 create mode 100644 perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp
 create mode 100644 perception/multi_object_tracker/src/debugger/debug_object.cpp

diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt
index 6adc0606add56..fb2c4d73554a3 100644
--- 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/debugger.cpp
+  src/debugger/debug_object.cpp
   src/processor/processor.cpp
   src/processor/input_manager.cpp
   src/data_association/data_association.cpp
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
new file mode 100644
index 0000000000000..18b8b77739800
--- /dev/null
+++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp
@@ -0,0 +1,59 @@
+// 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 <visualization_msgs/msg/marker_array.hpp>
+
+#include <list>
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <unordered_set>
+
+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_;
+
+public:
+  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 getMessage(visualization_msgs::msg::MarkerArray & message);
+};
+
+#endif  // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
index 2c10fabd2a91e..8a5ebd8a61ff1 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
@@ -11,12 +11,12 @@
 // 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__DEBUGGER_HPP_
 #define MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_
 
+#include "multi_object_tracker/debugger/debug_object.hpp"
+
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <diagnostic_updater/publisher.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -27,7 +27,10 @@
 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 
+#include <list>
 #include <memory>
+#include <string>
+#include <unordered_map>
 
 /**
  * @brief Debugger class for multi object tracker
@@ -36,7 +39,7 @@
 class TrackerDebugger
 {
 public:
-  explicit TrackerDebugger(rclcpp::Node & node);
+  explicit TrackerDebugger(rclcpp::Node & node, const std::string & frame_id);
 
 private:
   struct DEBUG_SETTINGS
@@ -47,13 +50,18 @@ class TrackerDebugger
     double diagnostics_error_delay;
   } debug_settings_;
 
+  // ROS node, publishers
   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::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_objects_markers_pub_;
 
   diagnostic_updater::Updater diagnostic_updater_;
+  // Object debugger
+  TrackerObjectDebugger object_debugger_;
 
+  // Time measurement
   bool is_initialized_ = false;
   double pipeline_latency_ms_ = 0.0;
   rclcpp::Time last_input_stamp_;
@@ -62,13 +70,17 @@ class TrackerDebugger
   rclcpp::Time stamp_publish_start_;
   rclcpp::Time stamp_publish_output_;
 
-public:
-  bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; }
+  // Configuration
   void setupDiagnostics();
+  void loadParameters();
 
+public:
+  // Object publishing
+  bool shouldPublishTentativeObjects() const { return debug_settings_.publish_tentative_objects; }
   void publishTentativeObjects(
     const autoware_auto_perception_msgs::msg::TrackedObjects & tentative_objects) const;
 
+  // Time measurement
   void startMeasurementTime(
     const rclcpp::Time & now, const rclcpp::Time & measurement_header_stamp);
   void endMeasurementTime(const rclcpp::Time & now);
@@ -76,8 +88,14 @@ class TrackerDebugger
   void endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time);
   void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
-private:
-  void loadParameters();
+  // Debug object
+  void collectObjectInfo(
+    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 publishObjectsMarkers();
 };
 
 #endif  // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUGGER_HPP_
diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp
new file mode 100644
index 0000000000000..7c317c002b754
--- /dev/null
+++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp
@@ -0,0 +1,200 @@
+// 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.
+
+#include "multi_object_tracker/debugger/debug_object.hpp"
+
+#include "autoware_auto_perception_msgs/msg/tracked_object.hpp"
+
+#include <boost/uuid/uuid.hpp>
+
+#include <functional>
+#include <string>
+
+namespace
+{
+int uuid_to_int(const unique_identifier_msgs::msg::UUID & uuid)
+{
+  // Convert UUID to string
+  std::string uuid_str;
+  for (auto byte : uuid.uuid) {
+    uuid_str += std::to_string(byte);
+  }
+
+  // Hash the string to get an int
+  std::hash<std::string> hasher;
+  int hashed_uuid = hasher(uuid_str);
+
+  return hashed_uuid;
+}
+}  // namespace
+
+TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id)
+{
+  // set frame id
+  frame_id_ = frame_id;
+
+  // initialize markers
+  markers_.markers.clear();
+  current_ids_.clear();
+  previous_ids_.clear();
+  message_time_ = rclcpp::Time(0, 0);
+}
+
+void TrackerObjectDebugger::reset()
+{
+  markers_.markers.clear();
+  current_ids_.clear();
+}
+
+void TrackerObjectDebugger::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*/)
+{
+  constexpr int PALETTE_SIZE = 32;
+  constexpr std::array<std::array<double, 3>, PALETTE_SIZE> color_array = {{
+    {{1.0, 0.0, 0.0}},    {{0.0, 1.0, 0.0}},
+    {{0.0, 0.0, 1.0}},  // Red, Green, Blue
+    {{1.0, 1.0, 0.0}},    {{0.0, 1.0, 1.0}},
+    {{1.0, 0.0, 1.0}},  // Yellow, Cyan, Magenta
+    {{1.0, 0.64, 0.0}},   {{0.75, 1.0, 0.0}},
+    {{0.0, 0.5, 0.5}},  // Orange, Lime, Teal
+    {{0.5, 0.0, 0.5}},    {{1.0, 0.75, 0.8}},
+    {{0.65, 0.17, 0.17}},  // Purple, Pink, Brown
+    {{0.5, 0.0, 0.0}},    {{0.5, 0.5, 0.0}},
+    {{0.0, 0.0, 0.5}},  // Maroon, Olive, Navy
+    {{0.5, 0.5, 0.5}},    {{1.0, 0.4, 0.4}},
+    {{0.4, 1.0, 0.4}},  // Grey, Light Red, Light Green
+    {{0.4, 0.4, 1.0}},    {{1.0, 1.0, 0.4}},
+    {{0.4, 1.0, 1.0}},  // Light Blue, Light Yellow, Light Cyan
+    {{1.0, 0.4, 1.0}},    {{1.0, 0.7, 0.4}},
+    {{0.7, 0.4, 1.0}},  // Light Magenta, Light Orange, Light Purple
+    {{1.0, 0.6, 0.8}},    {{0.71, 0.4, 0.114}},
+    {{0.55, 0.0, 0.0}},  // Light Pink, Light Brown, Dark Red
+    {{0.0, 0.4, 0.0}},    {{0.0, 0.0, 0.55}},
+    {{0.55, 0.55, 0.0}},                       // Dark Green, Dark Blue, Dark Yellow
+    {{0.0, 0.55, 0.55}},  {{0.55, 0.0, 0.55}}  // Dark Cyan, Dark Magenta
+  }};
+
+  message_time_ = message_time;
+  if (!is_initialized_) is_initialized_ = true;
+
+  for (const auto & tracker : list_tracker) {
+    // get object
+    autoware_auto_perception_msgs::msg::TrackedObject tracked_object;
+    tracker->getTrackedObject(message_time_, tracked_object);
+    const unique_identifier_msgs::msg::UUID uuid = tracked_object.object_id;
+    const int uuid_int = uuid_to_int(uuid);
+    current_ids_.emplace(uuid_int);
+
+    // get existence probability
+
+    // get color - by channel index
+    std_msgs::msg::ColorRGBA color;
+    color.a = 1.0;
+    color.r = color_array[channel_index % PALETTE_SIZE][0];
+    color.g = color_array[channel_index % PALETTE_SIZE][1];
+    color.b = color_array[channel_index % PALETTE_SIZE][2];
+
+    // get marker - box
+
+    visualization_msgs::msg::Marker marker;
+    marker.header.frame_id = frame_id_;
+    marker.header.stamp = message_time_;
+    marker.ns = "boxes";
+    marker.id = uuid_int;
+    marker.type = visualization_msgs::msg::Marker::CUBE;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    marker.pose.position.x = tracked_object.kinematics.pose_with_covariance.pose.position.x;
+    marker.pose.position.y = tracked_object.kinematics.pose_with_covariance.pose.position.y;
+    marker.pose.position.z = tracked_object.kinematics.pose_with_covariance.pose.position.z;
+    marker.scale.x = 0.3;
+    marker.scale.y = 0.3;
+    marker.scale.z = 0.3;
+
+    marker.color = color;
+    marker.lifetime = rclcpp::Duration::from_seconds(0);
+    markers_.markers.push_back(marker);
+
+    // get marker - existence probability text
+    std::vector<float> existence_vector;
+    tracker->getExistenceProbabilityVector(existence_vector);
+    std::string existence_probability_text = "P: ";
+    for (const auto & existence_probability : existence_vector) {
+      // probability to text, two digits of percentage
+      existence_probability_text +=
+        std::to_string(static_cast<int>(existence_probability * 100)) + " ";
+    }
+
+    visualization_msgs::msg::Marker text_marker;
+    text_marker.header.frame_id = frame_id_;
+    text_marker.header.stamp = message_time_;
+    text_marker.ns = "existence_probability";
+    text_marker.id = uuid_int;
+    text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+    text_marker.action = visualization_msgs::msg::Marker::ADD;
+    text_marker.pose.position.x = tracked_object.kinematics.pose_with_covariance.pose.position.x;
+    text_marker.pose.position.y = tracked_object.kinematics.pose_with_covariance.pose.position.y;
+    text_marker.pose.position.z =
+      tracked_object.kinematics.pose_with_covariance.pose.position.z + 0.8;
+    text_marker.scale.z = 0.8;
+    text_marker.color.a = 1.0;
+    text_marker.color.r = 1.0;
+    text_marker.color.g = 1.0;
+    text_marker.color.b = 1.0;
+    text_marker.text = existence_probability_text;
+    text_marker.lifetime = rclcpp::Duration::from_seconds(0);
+    markers_.markers.push_back(text_marker);
+
+    // get marker - association link lines
+  }
+}
+
+void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array)
+{
+  if (!is_initialized_) return;
+
+  // fill in marker array
+  for (auto & marker : markers_.markers) {
+    marker_array.markers.push_back(marker);
+  }
+
+  // remove old markers
+  for (const auto & previous_id : previous_ids_) {
+    if (current_ids_.find(previous_id) != current_ids_.end()) {
+      continue;
+    }
+
+    visualization_msgs::msg::Marker delete_marker;
+    delete_marker.header.frame_id = frame_id_;
+    delete_marker.header.stamp = message_time_;
+    delete_marker.ns = "boxes";
+    delete_marker.id = previous_id;
+    delete_marker.action = visualization_msgs::msg::Marker::DELETE;
+
+    marker_array.markers.push_back(delete_marker);
+
+    delete_marker.ns = "existence_probability";
+    marker_array.markers.push_back(delete_marker);
+  }
+
+  previous_ids_.clear();
+  previous_ids_ = current_ids_;
+  current_ids_.clear();
+  markers_.markers.clear();
+
+  return;
+}
diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp
index 30838220a566e..bfba05e587c9e 100644
--- a/perception/multi_object_tracker/src/debugger/debugger.cpp
+++ b/perception/multi_object_tracker/src/debugger/debugger.cpp
@@ -16,7 +16,8 @@
 
 #include <memory>
 
-TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : node_(node), diagnostic_updater_(&node)
+TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_id)
+: node_(node), diagnostic_updater_(&node), object_debugger_(frame_id)
 {
   // declare debug parameters to decide whether to publish debug topics
   loadParameters();
@@ -32,6 +33,9 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : node_(node), diagnostic_
         "debug/tentative_objects", rclcpp::QoS{1});
   }
 
+  debug_objects_markers_pub_ = node_.create_publisher<visualization_msgs::msg::MarkerArray>(
+    "multi_object_tracker/debug/objects_markers", rclcpp::QoS{1});
+
   // initialize timestamps
   const rclcpp::Time now = node_.now();
   last_input_stamp_ = now;
@@ -169,3 +173,23 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim
   }
   stamp_publish_output_ = now;
 }
+
+void TrackerDebugger::collectObjectInfo(
+  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)
+{
+  object_debugger_.collect(
+    message_time, list_tracker, channel_index, detected_objects, direct_assignment,
+    reverse_assignment);
+}
+
+// ObjectDebugger
+void TrackerDebugger::publishObjectsMarkers()
+{
+  visualization_msgs::msg::MarkerArray marker_message;
+  object_debugger_.getMessage(marker_message);
+  debug_objects_markers_pub_->publish(marker_message);
+}
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 74f3a24ee0347..a80f12442f75e 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -165,7 +165,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   }
 
   // Debugger
-  debugger_ = std::make_unique<TrackerDebugger>(*this);
+  debugger_ = std::make_unique<TrackerDebugger>(*this, world_frame_id_);
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
@@ -229,7 +229,7 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
   // process end
   debugger_->endMeasurementTime(this->now());
 
-  // for debug
+  /* DEBUG */
   const rclcpp::Time latest_time(objects_list.back().second.header.stamp);
   RCLCPP_INFO(
     this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f",
@@ -252,6 +252,7 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
       input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + "  ";
   }
   RCLCPP_INFO(this->get_logger(), object_counts_str.c_str());
+  /* DEBUG END */
 }
 
 void MultiObjectTracker::runProcess(
@@ -297,6 +298,12 @@ void MultiObjectTracker::runProcess(
 
   /* spawn new tracker */
   processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index);
+
+  // Collect debug information - tracker list, existence probabilities, association result
+  // TODO(technolojin): add option to enable/disable debug information
+  debugger_->collectObjectInfo(
+    measurement_time, processor_->getListTracker(), channel_index, transformed_objects,
+    direct_assignment, reverse_assignment);
 }
 
 void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time)
@@ -337,6 +344,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const
     processor_->getTentativeObjects(time, tentative_objects_msg);
     debugger_->publishTentativeObjects(tentative_objects_msg);
   }
+  debugger_->publishObjectsMarkers();
 }
 
 }  // namespace multi_object_tracker

From 15455cd26e74150a64de064b9e3f6053c82d2ab7 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 18 Apr 2024 19:18:59 +0900
Subject: [PATCH 27/67] feat: indexing marker step 1

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../debugger/debug_object.hpp                 | 90 ++++++++++++++++++-
 .../src/debugger/debug_object.cpp             | 47 ++++++++--
 .../src/debugger/debugger.cpp                 |  1 +
 3 files changed, 129 insertions(+), 9 deletions(-)

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
index 18b8b77739800..699cb100cb033 100644
--- 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
@@ -22,13 +22,39 @@
 
 #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
+  unique_identifier_msgs::msg::UUID uuid;
+
+  // association link, pair of coordinates
+  // tracker to detection
+  geometry_msgs::msg::Point tracker_point;
+  geometry_msgs::msg::Point detection_point;
+
+  // existence probabilities
+  std::vector<float> existence_vector;
+
+  // detection channel id
+  uint channel_id;
+};
 
 class TrackerObjectDebugger
 {
@@ -43,6 +69,11 @@ class TrackerObjectDebugger
   std::unordered_set<int> previous_ids_;
   rclcpp::Time message_time_;
 
+  std::unordered_map<int, ObjectData> object_data_;
+  std::unordered_map<boost::uuids::uuid, int32_t, boost::hash<boost::uuids::uuid>> id_map_;
+  std::list<int32_t> unused_marker_ids_;
+  int32_t marker_id_ = 0;
+
 public:
   void collect(
     const rclcpp::Time & message_time, const std::list<std::shared_ptr<Tracker>> & list_tracker,
@@ -52,8 +83,65 @@ class TrackerObjectDebugger
     const std::unordered_map<int, int> & reverse_assignment);
 
   void reset();
+  void draw();
+  void getMessage(visualization_msgs::msg::MarkerArray & message) const;
+
+private:
+  std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const
+  {
+    std::stringstream ss;
+    for (auto i = 0; i < 16; ++i) {
+      ss << std::hex << std::setfill('0') << std::setw(2) << +u.uuid[i];
+    }
+    return ss.str();
+  }
+
+  boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg)
+  {
+    const std::string uuid_str = uuid_to_string(uuid_msg);
+    boost::uuids::string_generator gen;
+    boost::uuids::uuid uuid = gen(uuid_str);
+    return uuid;
+  }
+
+  void update_id_map(const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & msg)
+  {
+    std::vector<boost::uuids::uuid> new_uuids;
+    std::vector<boost::uuids::uuid> tracked_uuids;
+    new_uuids.reserve(msg->objects.size());
+    tracked_uuids.reserve(msg->objects.size());
+    for (const auto & object : msg->objects) {
+      const auto uuid = to_boost_uuid(object.object_id);
+      ((id_map_.find(uuid) != id_map_.end()) ? tracked_uuids : new_uuids).push_back(uuid);
+    }
+
+    auto itr = id_map_.begin();
+    while (itr != id_map_.end()) {
+      if (
+        std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) {
+        unused_marker_ids_.push_back(itr->second);
+        itr = id_map_.erase(itr);
+      } else {
+        ++itr;
+      }
+    }
+
+    for (const auto & new_uuid : new_uuids) {
+      if (unused_marker_ids_.empty()) {
+        id_map_.emplace(new_uuid, marker_id_);
+        marker_id_++;
+      } else {
+        id_map_.emplace(new_uuid, unused_marker_ids_.front());
+        unused_marker_ids_.pop_front();
+      }
+    }
+  }
 
-  void getMessage(visualization_msgs::msg::MarkerArray & message);
+  int32_t uuid_to_marker_id(const unique_identifier_msgs::msg::UUID & uuid_msg)
+  {
+    auto uuid = to_boost_uuid(uuid_msg);
+    return id_map_.at(uuid);
+  }
 };
 
 #endif  // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_
diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp
index 7c317c002b754..1588abad41f09 100644
--- a/perception/multi_object_tracker/src/debugger/debug_object.cpp
+++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp
@@ -37,6 +37,7 @@ int uuid_to_int(const unique_identifier_msgs::msg::UUID & uuid)
 
   return hashed_uuid;
 }
+
 }  // namespace
 
 TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id)
@@ -53,8 +54,10 @@ TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id)
 
 void TrackerObjectDebugger::reset()
 {
-  markers_.markers.clear();
+  previous_ids_.clear();
+  previous_ids_ = current_ids_;
   current_ids_.clear();
+  markers_.markers.clear();
 }
 
 void TrackerObjectDebugger::collect(
@@ -82,7 +85,7 @@ void TrackerObjectDebugger::collect(
     {{0.4, 1.0, 1.0}},  // Light Blue, Light Yellow, Light Cyan
     {{1.0, 0.4, 1.0}},    {{1.0, 0.7, 0.4}},
     {{0.7, 0.4, 1.0}},  // Light Magenta, Light Orange, Light Purple
-    {{1.0, 0.6, 0.8}},    {{0.71, 0.4, 0.114}},
+    {{1.0, 0.6, 0.8}},    {{0.71, 0.4, 0.12}},
     {{0.55, 0.0, 0.0}},  // Light Pink, Light Brown, Dark Red
     {{0.0, 0.4, 0.0}},    {{0.0, 0.0, 0.55}},
     {{0.55, 0.55, 0.0}},                       // Dark Green, Dark Blue, Dark Yellow
@@ -163,10 +166,43 @@ void TrackerObjectDebugger::collect(
   }
 }
 
-void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array)
+void TrackerObjectDebugger::draw()
+{
+  // constexpr int PALETTE_SIZE = 32;
+  // constexpr std::array<std::array<double, 3>, PALETTE_SIZE> color_array = {{
+  //   {{1.0, 0.0, 0.0}},    {{0.0, 1.0, 0.0}},
+  //   {{0.0, 0.0, 1.0}},  // Red, Green, Blue
+  //   {{1.0, 1.0, 0.0}},    {{0.0, 1.0, 1.0}},
+  //   {{1.0, 0.0, 1.0}},  // Yellow, Cyan, Magenta
+  //   {{1.0, 0.64, 0.0}},   {{0.75, 1.0, 0.0}},
+  //   {{0.0, 0.5, 0.5}},  // Orange, Lime, Teal
+  //   {{0.5, 0.0, 0.5}},    {{1.0, 0.75, 0.8}},
+  //   {{0.65, 0.17, 0.17}},  // Purple, Pink, Brown
+  //   {{0.5, 0.0, 0.0}},    {{0.5, 0.5, 0.0}},
+  //   {{0.0, 0.0, 0.5}},  // Maroon, Olive, Navy
+  //   {{0.5, 0.5, 0.5}},    {{1.0, 0.4, 0.4}},
+  //   {{0.4, 1.0, 0.4}},  // Grey, Light Red, Light Green
+  //   {{0.4, 0.4, 1.0}},    {{1.0, 1.0, 0.4}},
+  //   {{0.4, 1.0, 1.0}},  // Light Blue, Light Yellow, Light Cyan
+  //   {{1.0, 0.4, 1.0}},    {{1.0, 0.7, 0.4}},
+  //   {{0.7, 0.4, 1.0}},  // Light Magenta, Light Orange, Light Purple
+  //   {{1.0, 0.6, 0.8}},    {{0.71, 0.4, 0.12}},
+  //   {{0.55, 0.0, 0.0}},  // Light Pink, Light Brown, Dark Red
+  //   {{0.0, 0.4, 0.0}},    {{0.0, 0.0, 0.55}},
+  //   {{0.55, 0.55, 0.0}},                       // Dark Green, Dark Blue, Dark Yellow
+  //   {{0.0, 0.55, 0.55}},  {{0.55, 0.0, 0.55}}  // Dark Cyan, Dark Magenta
+  // }};
+
+  // do nothing for now
+  return;
+}
+
+void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array) const
 {
   if (!is_initialized_) return;
 
+  // draw markers
+
   // fill in marker array
   for (auto & marker : markers_.markers) {
     marker_array.markers.push_back(marker);
@@ -191,10 +227,5 @@ void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & ma
     marker_array.markers.push_back(delete_marker);
   }
 
-  previous_ids_.clear();
-  previous_ids_ = current_ids_;
-  current_ids_.clear();
-  markers_.markers.clear();
-
   return;
 }
diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp
index bfba05e587c9e..4d0d747bc2ef1 100644
--- a/perception/multi_object_tracker/src/debugger/debugger.cpp
+++ b/perception/multi_object_tracker/src/debugger/debugger.cpp
@@ -192,4 +192,5 @@ void TrackerDebugger::publishObjectsMarkers()
   visualization_msgs::msg::MarkerArray marker_message;
   object_debugger_.getMessage(marker_message);
   debug_objects_markers_pub_->publish(marker_message);
+  object_debugger_.reset();
 }

From 212752c1173fa0d100e71990976bc515dab91518 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 19 Apr 2024 11:47:44 +0900
Subject: [PATCH 28/67] fix: uuid management

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../debugger/debug_object.hpp                 |  31 +-
 .../src/debugger/debug_object.cpp             | 276 ++++++++++--------
 2 files changed, 181 insertions(+), 126 deletions(-)

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
index 699cb100cb033..707cfd4a475aa 100644
--- 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
@@ -42,12 +42,14 @@ struct ObjectData
   rclcpp::Time time;
 
   // object uuid
-  unique_identifier_msgs::msg::UUID uuid;
+  boost::uuids::uuid uuid;
+  int uuid_int;
 
   // 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;
@@ -69,7 +71,7 @@ class TrackerObjectDebugger
   std::unordered_set<int> previous_ids_;
   rclcpp::Time message_time_;
 
-  std::unordered_map<int, ObjectData> object_data_;
+  std::vector<ObjectData> object_data_list_;
   std::unordered_map<boost::uuids::uuid, int32_t, boost::hash<boost::uuids::uuid>> id_map_;
   std::list<int32_t> unused_marker_ids_;
   int32_t marker_id_ = 0;
@@ -83,8 +85,10 @@ class TrackerObjectDebugger
     const std::unordered_map<int, int> & reverse_assignment);
 
   void reset();
-  void draw();
-  void getMessage(visualization_msgs::msg::MarkerArray & message) const;
+  void draw(
+    const std::vector<std::vector<ObjectData>> object_data_groups,
+    visualization_msgs::msg::MarkerArray & marker_array) const;
+  void getMessage(visualization_msgs::msg::MarkerArray & marker_array);
 
 private:
   std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const
@@ -104,17 +108,20 @@ class TrackerObjectDebugger
     return uuid;
   }
 
-  void update_id_map(const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & msg)
+  void update_id_map(const std::vector<ObjectData> & object_data_list)
   {
     std::vector<boost::uuids::uuid> new_uuids;
     std::vector<boost::uuids::uuid> tracked_uuids;
-    new_uuids.reserve(msg->objects.size());
-    tracked_uuids.reserve(msg->objects.size());
-    for (const auto & object : msg->objects) {
-      const auto uuid = to_boost_uuid(object.object_id);
-      ((id_map_.find(uuid) != id_map_.end()) ? tracked_uuids : new_uuids).push_back(uuid);
+    new_uuids.reserve(object_data_list.size());
+    tracked_uuids.reserve(object_data_list.size());
+
+    // check if the object is already tracked
+    for (const auto & object_data : object_data_list) {
+      ((id_map_.find(object_data.uuid) != id_map_.end()) ? tracked_uuids : new_uuids)
+        .push_back(object_data.uuid);
     }
 
+    // remove untracked objects
     auto itr = id_map_.begin();
     while (itr != id_map_.end()) {
       if (
@@ -126,6 +133,7 @@ class TrackerObjectDebugger
       }
     }
 
+    // add new objects
     for (const auto & new_uuid : new_uuids) {
       if (unused_marker_ids_.empty()) {
         id_map_.emplace(new_uuid, marker_id_);
@@ -139,9 +147,10 @@ class TrackerObjectDebugger
 
   int32_t uuid_to_marker_id(const unique_identifier_msgs::msg::UUID & uuid_msg)
   {
-    auto uuid = to_boost_uuid(uuid_msg);
+    const auto uuid = to_boost_uuid(uuid_msg);
     return id_map_.at(uuid);
   }
+  int32_t uuid_to_marker_id(const boost::uuids::uuid & uuid) { return id_map_.at(uuid); }
 };
 
 #endif  // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_
diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp
index 1588abad41f09..c6f19235dda11 100644
--- a/perception/multi_object_tracker/src/debugger/debug_object.cpp
+++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp
@@ -21,25 +21,6 @@
 #include <functional>
 #include <string>
 
-namespace
-{
-int uuid_to_int(const unique_identifier_msgs::msg::UUID & uuid)
-{
-  // Convert UUID to string
-  std::string uuid_str;
-  for (auto byte : uuid.uuid) {
-    uuid_str += std::to_string(byte);
-  }
-
-  // Hash the string to get an int
-  std::hash<std::string> hasher;
-  int hashed_uuid = hasher(uuid_str);
-
-  return hashed_uuid;
-}
-
-}  // namespace
-
 TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id)
 {
   // set frame id
@@ -54,19 +35,100 @@ TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id)
 
 void TrackerObjectDebugger::reset()
 {
+  // maintain previous ids
   previous_ids_.clear();
   previous_ids_ = current_ids_;
   current_ids_.clear();
+
+  // clear markers, object data list
+  object_data_list_.clear();
   markers_.markers.clear();
 }
 
 void TrackerObjectDebugger::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 autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects,
+  const std::unordered_map<int, int> & direct_assignment,
   const std::unordered_map<int, int> & /*reverse_assignment*/)
 {
+  if (!is_initialized_) is_initialized_ = true;
+
+  message_time_ = message_time;
+
+  int tracker_idx = 0;
+  for (auto tracker_itr = list_tracker.begin(); tracker_itr != list_tracker.end();
+       ++tracker_itr, ++tracker_idx) {
+    ObjectData object_data;
+    object_data.time = message_time;
+    object_data.channel_id = channel_index;
+
+    autoware_auto_perception_msgs::msg::TrackedObject tracked_object;
+    (*(tracker_itr))->getTrackedObject(message_time, tracked_object);
+    object_data.uuid = to_boost_uuid(tracked_object.object_id);
+
+    // tracker
+    bool is_associated = false;
+    geometry_msgs::msg::Point tracker_point, detection_point;
+    tracker_point.x = tracked_object.kinematics.pose_with_covariance.pose.position.x;
+    tracker_point.y = tracked_object.kinematics.pose_with_covariance.pose.position.y;
+    tracker_point.z = tracked_object.kinematics.pose_with_covariance.pose.position.z;
+
+    // detection
+    if (direct_assignment.find(tracker_idx) != direct_assignment.end()) {
+      const auto & associated_object =
+        detected_objects.objects.at(direct_assignment.find(tracker_idx)->second);
+      detection_point.x = associated_object.kinematics.pose_with_covariance.pose.position.x;
+      detection_point.y = associated_object.kinematics.pose_with_covariance.pose.position.y;
+      detection_point.z = associated_object.kinematics.pose_with_covariance.pose.position.z;
+      is_associated = true;
+    }
+
+    object_data.tracker_point = tracker_point;
+    object_data.detection_point = detection_point;
+    object_data.is_associated = is_associated;
+
+    // existence probabilities
+    std::vector<float> existence_vector;
+    (*(tracker_itr))->getExistenceProbabilityVector(existence_vector);
+    object_data.existence_vector = existence_vector;
+
+    object_data_list_.push_back(object_data);
+  }
+
+  // constexpr int PALETTE_SIZE = 32;
+  // constexpr std::array<std::array<double, 3>, PALETTE_SIZE> color_array = {{
+  //   {{1.0, 0.0, 0.0}},    {{0.0, 1.0, 0.0}},
+  //   {{0.0, 0.0, 1.0}},  // Red, Green, Blue
+  //   {{1.0, 1.0, 0.0}},    {{0.0, 1.0, 1.0}},
+  //   {{1.0, 0.0, 1.0}},  // Yellow, Cyan, Magenta
+  //   {{1.0, 0.64, 0.0}},   {{0.75, 1.0, 0.0}},
+  //   {{0.0, 0.5, 0.5}},  // Orange, Lime, Teal
+  //   {{0.5, 0.0, 0.5}},    {{1.0, 0.75, 0.8}},
+  //   {{0.65, 0.17, 0.17}},  // Purple, Pink, Brown
+  //   {{0.5, 0.0, 0.0}},    {{0.5, 0.5, 0.0}},
+  //   {{0.0, 0.0, 0.5}},  // Maroon, Olive, Navy
+  //   {{0.5, 0.5, 0.5}},    {{1.0, 0.4, 0.4}},
+  //   {{0.4, 1.0, 0.4}},  // Grey, Light Red, Light Green
+  //   {{0.4, 0.4, 1.0}},    {{1.0, 1.0, 0.4}},
+  //   {{0.4, 1.0, 1.0}},  // Light Blue, Light Yellow, Light Cyan
+  //   {{1.0, 0.4, 1.0}},    {{1.0, 0.7, 0.4}},
+  //   {{0.7, 0.4, 1.0}},  // Light Magenta, Light Orange, Light Purple
+  //   {{1.0, 0.6, 0.8}},    {{0.71, 0.4, 0.12}},
+  //   {{0.55, 0.0, 0.0}},  // Light Pink, Light Brown, Dark Red
+  //   {{0.0, 0.4, 0.0}},    {{0.0, 0.0, 0.55}},
+  //   {{0.55, 0.55, 0.0}},                       // Dark Green, Dark Blue, Dark Yellow
+  //   {{0.0, 0.55, 0.55}},  {{0.55, 0.0, 0.55}}  // Dark Cyan, Dark Magenta
+  // }};
+}
+
+void TrackerObjectDebugger::draw(
+  const std::vector<std::vector<ObjectData>> object_data_groups,
+  visualization_msgs::msg::MarkerArray & marker_array) const
+{
+  // initialize markers
+  marker_array.markers.clear();
+
   constexpr int PALETTE_SIZE = 32;
   constexpr std::array<std::array<double, 3>, PALETTE_SIZE> color_array = {{
     {{1.0, 0.0, 0.0}},    {{0.0, 1.0, 0.0}},
@@ -92,122 +154,106 @@ void TrackerObjectDebugger::collect(
     {{0.0, 0.55, 0.55}},  {{0.55, 0.0, 0.55}}  // Dark Cyan, Dark Magenta
   }};
 
-  message_time_ = message_time;
-  if (!is_initialized_) is_initialized_ = true;
-
-  for (const auto & tracker : list_tracker) {
-    // get object
-    autoware_auto_perception_msgs::msg::TrackedObject tracked_object;
-    tracker->getTrackedObject(message_time_, tracked_object);
-    const unique_identifier_msgs::msg::UUID uuid = tracked_object.object_id;
-    const int uuid_int = uuid_to_int(uuid);
-    current_ids_.emplace(uuid_int);
-
-    // get existence probability
-
-    // get color - by channel index
-    std_msgs::msg::ColorRGBA color;
-    color.a = 1.0;
-    color.r = color_array[channel_index % PALETTE_SIZE][0];
-    color.g = color_array[channel_index % PALETTE_SIZE][1];
-    color.b = color_array[channel_index % PALETTE_SIZE][2];
-
-    // get marker - box
-
+  for (const auto & object_data_group : object_data_groups) {
+    if (object_data_group.empty()) continue;
+    const int & uuid_int = object_data_group.front().uuid_int;
+    const auto object_data_front = object_data_group.front();
+    const auto object_data_back = object_data_group.back();
+    // set a reference marker
     visualization_msgs::msg::Marker marker;
     marker.header.frame_id = frame_id_;
-    marker.header.stamp = message_time_;
-    marker.ns = "boxes";
+    marker.header.stamp = object_data_front.time;
     marker.id = uuid_int;
-    marker.type = visualization_msgs::msg::Marker::CUBE;
-    marker.action = visualization_msgs::msg::Marker::ADD;
-    marker.pose.position.x = tracked_object.kinematics.pose_with_covariance.pose.position.x;
-    marker.pose.position.y = tracked_object.kinematics.pose_with_covariance.pose.position.y;
-    marker.pose.position.z = tracked_object.kinematics.pose_with_covariance.pose.position.z;
-    marker.scale.x = 0.3;
-    marker.scale.y = 0.3;
-    marker.scale.z = 0.3;
-
-    marker.color = color;
+    marker.pose.position.x = object_data_front.tracker_point.x;
+    marker.pose.position.y = object_data_front.tracker_point.y;
+    marker.pose.position.z = object_data_front.tracker_point.z;
+    marker.color.a = 1.0;
+    marker.color.r = 1.0;
+    marker.color.g = 1.0;
+    marker.color.b = 1.0;  // white
     marker.lifetime = rclcpp::Duration::from_seconds(0);
-    markers_.markers.push_back(marker);
-
-    // get marker - existence probability text
-    std::vector<float> existence_vector;
-    tracker->getExistenceProbabilityVector(existence_vector);
-    std::string existence_probability_text = "P: ";
-    for (const auto & existence_probability : existence_vector) {
-      // probability to text, two digits of percentage
-      existence_probability_text +=
-        std::to_string(static_cast<int>(existence_probability * 100)) + " ";
-    }
 
+    // get marker - existence_probability
     visualization_msgs::msg::Marker text_marker;
-    text_marker.header.frame_id = frame_id_;
-    text_marker.header.stamp = message_time_;
+    text_marker = marker;
     text_marker.ns = "existence_probability";
-    text_marker.id = uuid_int;
     text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
     text_marker.action = visualization_msgs::msg::Marker::ADD;
-    text_marker.pose.position.x = tracked_object.kinematics.pose_with_covariance.pose.position.x;
-    text_marker.pose.position.y = tracked_object.kinematics.pose_with_covariance.pose.position.y;
-    text_marker.pose.position.z =
-      tracked_object.kinematics.pose_with_covariance.pose.position.z + 0.8;
+    text_marker.pose.position.z += 1.5;
     text_marker.scale.z = 0.8;
-    text_marker.color.a = 1.0;
-    text_marker.color.r = 1.0;
-    text_marker.color.g = 1.0;
-    text_marker.color.b = 1.0;
-    text_marker.text = existence_probability_text;
-    text_marker.lifetime = rclcpp::Duration::from_seconds(0);
-    markers_.markers.push_back(text_marker);
 
-    // get marker - association link lines
+    // show the last existence probability
+    std::string existence_probability_text = "P: ";
+    for (const auto & existence_probability : object_data_front.existence_vector) {
+      // probability to text, two digits of percentage
+      existence_probability_text +=
+        std::to_string(static_cast<int>(existence_probability * 100)) + " ";
+    }
+    text_marker.text = existence_probability_text;
+    marker_array.markers.push_back(text_marker);
+
+    // loop for each object_data in the group
+    // boxed to tracker positions
+    // and link lines to the detected positions
+    visualization_msgs::msg::Marker marker_boxes;
+    marker_boxes = marker;
+    marker_boxes.ns = "boxes";
+    marker_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST;
+    marker_boxes.action = visualization_msgs::msg::Marker::ADD;
+
+    for (const auto & object_data : object_data_group) {
+      // get color - by channel index
+      std_msgs::msg::ColorRGBA color;
+      color.a = 1.0;
+      color.r = color_array[object_data.channel_id % PALETTE_SIZE][0];
+      color.g = color_array[object_data.channel_id % PALETTE_SIZE][1];
+      color.b = color_array[object_data.channel_id % PALETTE_SIZE][2];
+      marker_boxes.color = color;
+    }
   }
-}
-
-void TrackerObjectDebugger::draw()
-{
-  // constexpr int PALETTE_SIZE = 32;
-  // constexpr std::array<std::array<double, 3>, PALETTE_SIZE> color_array = {{
-  //   {{1.0, 0.0, 0.0}},    {{0.0, 1.0, 0.0}},
-  //   {{0.0, 0.0, 1.0}},  // Red, Green, Blue
-  //   {{1.0, 1.0, 0.0}},    {{0.0, 1.0, 1.0}},
-  //   {{1.0, 0.0, 1.0}},  // Yellow, Cyan, Magenta
-  //   {{1.0, 0.64, 0.0}},   {{0.75, 1.0, 0.0}},
-  //   {{0.0, 0.5, 0.5}},  // Orange, Lime, Teal
-  //   {{0.5, 0.0, 0.5}},    {{1.0, 0.75, 0.8}},
-  //   {{0.65, 0.17, 0.17}},  // Purple, Pink, Brown
-  //   {{0.5, 0.0, 0.0}},    {{0.5, 0.5, 0.0}},
-  //   {{0.0, 0.0, 0.5}},  // Maroon, Olive, Navy
-  //   {{0.5, 0.5, 0.5}},    {{1.0, 0.4, 0.4}},
-  //   {{0.4, 1.0, 0.4}},  // Grey, Light Red, Light Green
-  //   {{0.4, 0.4, 1.0}},    {{1.0, 1.0, 0.4}},
-  //   {{0.4, 1.0, 1.0}},  // Light Blue, Light Yellow, Light Cyan
-  //   {{1.0, 0.4, 1.0}},    {{1.0, 0.7, 0.4}},
-  //   {{0.7, 0.4, 1.0}},  // Light Magenta, Light Orange, Light Purple
-  //   {{1.0, 0.6, 0.8}},    {{0.71, 0.4, 0.12}},
-  //   {{0.55, 0.0, 0.0}},  // Light Pink, Light Brown, Dark Red
-  //   {{0.0, 0.4, 0.0}},    {{0.0, 0.0, 0.55}},
-  //   {{0.55, 0.55, 0.0}},                       // Dark Green, Dark Blue, Dark Yellow
-  //   {{0.0, 0.55, 0.55}},  {{0.55, 0.0, 0.55}}  // Dark Cyan, Dark Magenta
-  // }};
 
-  // do nothing for now
   return;
 }
 
-void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array) const
+void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array)
 {
   if (!is_initialized_) return;
 
-  // draw markers
+  // check all object data and update marker index
+  update_id_map(object_data_list_);
 
-  // fill in marker array
-  for (auto & marker : markers_.markers) {
-    marker_array.markers.push_back(marker);
+  // update uuid_int
+  for (auto & object_data : object_data_list_) {
+    object_data.uuid_int = uuid_to_marker_id(object_data.uuid);
   }
 
+  // sort by uuid, collect the same uuid object_data as a group, and loop for the groups
+  std::vector<std::vector<ObjectData>> object_data_groups;
+  {
+    // sort by uuid_int
+    std::sort(
+      object_data_list_.begin(), object_data_list_.end(),
+      [](const ObjectData & a, const ObjectData & b) { return a.uuid_int < b.uuid_int; });
+
+    // collect the same uuid object_data as a group
+    std::vector<ObjectData> object_data_group;
+    boost::uuids::uuid previous_uuid = object_data_list_.front().uuid;
+    for (const auto & object_data : object_data_list_) {
+      if (object_data.uuid != previous_uuid) {
+        object_data_groups.push_back(object_data_group);
+        object_data_group.clear();
+        previous_uuid = object_data.uuid;
+      }
+      object_data_group.push_back(object_data);
+    }
+
+    // fill the vector of groups
+    object_data_groups.push_back(object_data_group);
+  }
+
+  // draw markers
+  draw(object_data_groups, marker_array);
+
   // remove old markers
   for (const auto & previous_id : previous_ids_) {
     if (current_ids_.find(previous_id) != current_ids_.end()) {

From b6f59a9756cdcf8e1f2f4414422bd2ae97515ba8 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 19 Apr 2024 16:07:09 +0900
Subject: [PATCH 29/67] feat: association line fix

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../debugger/debug_object.hpp                 |   4 +-
 .../src/debugger/debug_object.cpp             | 203 ++++++++++--------
 .../src/debugger/debugger.cpp                 |   7 +
 3 files changed, 128 insertions(+), 86 deletions(-)

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
index 707cfd4a475aa..4875b6105fa9a 100644
--- 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
@@ -75,6 +75,7 @@ class TrackerObjectDebugger
   std::unordered_map<boost::uuids::uuid, int32_t, boost::hash<boost::uuids::uuid>> id_map_;
   std::list<int32_t> unused_marker_ids_;
   int32_t marker_id_ = 0;
+  std::vector<std::vector<ObjectData>> object_data_groups_;
 
 public:
   void collect(
@@ -88,7 +89,8 @@ class TrackerObjectDebugger
   void draw(
     const std::vector<std::vector<ObjectData>> object_data_groups,
     visualization_msgs::msg::MarkerArray & marker_array) const;
-  void getMessage(visualization_msgs::msg::MarkerArray & marker_array);
+  void process();
+  void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const;
 
 private:
   std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const
diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp
index c6f19235dda11..b7696ed7e7a44 100644
--- a/perception/multi_object_tracker/src/debugger/debug_object.cpp
+++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp
@@ -82,6 +82,10 @@ void TrackerObjectDebugger::collect(
       detection_point.y = associated_object.kinematics.pose_with_covariance.pose.position.y;
       detection_point.z = associated_object.kinematics.pose_with_covariance.pose.position.z;
       is_associated = true;
+    } else {
+      detection_point.x = tracker_point.x;
+      detection_point.y = tracker_point.y;
+      detection_point.z = tracker_point.z;
     }
 
     object_data.tracker_point = tracker_point;
@@ -95,31 +99,47 @@ void TrackerObjectDebugger::collect(
 
     object_data_list_.push_back(object_data);
   }
+}
+
+void TrackerObjectDebugger::process()
+{
+  if (!is_initialized_) return;
+
+  // check all object data and update marker index
+  update_id_map(object_data_list_);
+
+  // update uuid_int
+  for (auto & object_data : object_data_list_) {
+    object_data.uuid_int = uuid_to_marker_id(object_data.uuid);
+    current_ids_.insert(object_data.uuid_int);
+  }
+
+  // sort by uuid, collect the same uuid object_data as a group, and loop for the groups
+  // std::vector<std::vector<ObjectData>> object_data_groups;
+  object_data_groups_.clear();
+  {
+    // sort by uuid_int
+    std::sort(
+      object_data_list_.begin(), object_data_list_.end(),
+      [](const ObjectData & a, const ObjectData & b) { return a.uuid_int < b.uuid_int; });
 
-  // constexpr int PALETTE_SIZE = 32;
-  // constexpr std::array<std::array<double, 3>, PALETTE_SIZE> color_array = {{
-  //   {{1.0, 0.0, 0.0}},    {{0.0, 1.0, 0.0}},
-  //   {{0.0, 0.0, 1.0}},  // Red, Green, Blue
-  //   {{1.0, 1.0, 0.0}},    {{0.0, 1.0, 1.0}},
-  //   {{1.0, 0.0, 1.0}},  // Yellow, Cyan, Magenta
-  //   {{1.0, 0.64, 0.0}},   {{0.75, 1.0, 0.0}},
-  //   {{0.0, 0.5, 0.5}},  // Orange, Lime, Teal
-  //   {{0.5, 0.0, 0.5}},    {{1.0, 0.75, 0.8}},
-  //   {{0.65, 0.17, 0.17}},  // Purple, Pink, Brown
-  //   {{0.5, 0.0, 0.0}},    {{0.5, 0.5, 0.0}},
-  //   {{0.0, 0.0, 0.5}},  // Maroon, Olive, Navy
-  //   {{0.5, 0.5, 0.5}},    {{1.0, 0.4, 0.4}},
-  //   {{0.4, 1.0, 0.4}},  // Grey, Light Red, Light Green
-  //   {{0.4, 0.4, 1.0}},    {{1.0, 1.0, 0.4}},
-  //   {{0.4, 1.0, 1.0}},  // Light Blue, Light Yellow, Light Cyan
-  //   {{1.0, 0.4, 1.0}},    {{1.0, 0.7, 0.4}},
-  //   {{0.7, 0.4, 1.0}},  // Light Magenta, Light Orange, Light Purple
-  //   {{1.0, 0.6, 0.8}},    {{0.71, 0.4, 0.12}},
-  //   {{0.55, 0.0, 0.0}},  // Light Pink, Light Brown, Dark Red
-  //   {{0.0, 0.4, 0.0}},    {{0.0, 0.0, 0.55}},
-  //   {{0.55, 0.55, 0.0}},                       // Dark Green, Dark Blue, Dark Yellow
-  //   {{0.0, 0.55, 0.55}},  {{0.55, 0.0, 0.55}}  // Dark Cyan, Dark Magenta
-  // }};
+    // collect the same uuid object_data as a group
+    std::vector<ObjectData> object_data_group;
+    boost::uuids::uuid previous_uuid = object_data_list_.front().uuid;
+    for (const auto & object_data : object_data_list_) {
+      // if the uuid is different, push the group and clear the group
+      if (object_data.uuid != previous_uuid) {
+        // push the group
+        object_data_groups_.push_back(object_data_group);
+        object_data_group.clear();
+        previous_uuid = object_data.uuid;
+      }
+      // push the object_data to the group
+      object_data_group.push_back(object_data);
+    }
+    // push the last group
+    object_data_groups_.push_back(object_data_group);
+  }
 }
 
 void TrackerObjectDebugger::draw(
@@ -129,29 +149,24 @@ void TrackerObjectDebugger::draw(
   // initialize markers
   marker_array.markers.clear();
 
-  constexpr int PALETTE_SIZE = 32;
+  constexpr int PALETTE_SIZE = 16;
   constexpr std::array<std::array<double, 3>, PALETTE_SIZE> color_array = {{
-    {{1.0, 0.0, 0.0}},    {{0.0, 1.0, 0.0}},
-    {{0.0, 0.0, 1.0}},  // Red, Green, Blue
-    {{1.0, 1.0, 0.0}},    {{0.0, 1.0, 1.0}},
-    {{1.0, 0.0, 1.0}},  // Yellow, Cyan, Magenta
-    {{1.0, 0.64, 0.0}},   {{0.75, 1.0, 0.0}},
-    {{0.0, 0.5, 0.5}},  // Orange, Lime, Teal
-    {{0.5, 0.0, 0.5}},    {{1.0, 0.75, 0.8}},
-    {{0.65, 0.17, 0.17}},  // Purple, Pink, Brown
-    {{0.5, 0.0, 0.0}},    {{0.5, 0.5, 0.0}},
-    {{0.0, 0.0, 0.5}},  // Maroon, Olive, Navy
-    {{0.5, 0.5, 0.5}},    {{1.0, 0.4, 0.4}},
-    {{0.4, 1.0, 0.4}},  // Grey, Light Red, Light Green
-    {{0.4, 0.4, 1.0}},    {{1.0, 1.0, 0.4}},
-    {{0.4, 1.0, 1.0}},  // Light Blue, Light Yellow, Light Cyan
-    {{1.0, 0.4, 1.0}},    {{1.0, 0.7, 0.4}},
-    {{0.7, 0.4, 1.0}},  // Light Magenta, Light Orange, Light Purple
-    {{1.0, 0.6, 0.8}},    {{0.71, 0.4, 0.12}},
-    {{0.55, 0.0, 0.0}},  // Light Pink, Light Brown, Dark Red
-    {{0.0, 0.4, 0.0}},    {{0.0, 0.0, 0.55}},
-    {{0.55, 0.55, 0.0}},                       // Dark Green, Dark Blue, Dark Yellow
-    {{0.0, 0.55, 0.55}},  {{0.55, 0.0, 0.55}}  // Dark Cyan, Dark Magenta
+    {{0.0, 0.0, 1.0}},     // Blue
+    {{0.0, 1.0, 0.0}},     // Green
+    {{1.0, 1.0, 0.0}},     // Yellow
+    {{1.0, 0.0, 0.0}},     // Red
+    {{0.0, 1.0, 1.0}},     // Cyan
+    {{1.0, 0.0, 1.0}},     // Magenta
+    {{1.0, 0.64, 0.0}},    // Orange
+    {{0.75, 1.0, 0.0}},    // Lime
+    {{0.0, 0.5, 0.5}},     // Teal
+    {{0.5, 0.0, 0.5}},     // Purple
+    {{1.0, 0.75, 0.8}},    // Pink
+    {{0.65, 0.17, 0.17}},  // Brown
+    {{0.5, 0.0, 0.0}},     // Maroon
+    {{0.5, 0.5, 0.0}},     // Olive
+    {{0.0, 0.0, 0.5}},     // Navy
+    {{0.5, 0.5, 0.5}}      // Grey
   }};
 
   for (const auto & object_data_group : object_data_groups) {
@@ -164,9 +179,9 @@ void TrackerObjectDebugger::draw(
     marker.header.frame_id = frame_id_;
     marker.header.stamp = object_data_front.time;
     marker.id = uuid_int;
-    marker.pose.position.x = object_data_front.tracker_point.x;
-    marker.pose.position.y = object_data_front.tracker_point.y;
-    marker.pose.position.z = object_data_front.tracker_point.z;
+    marker.pose.position.x = 0;
+    marker.pose.position.y = 0;
+    marker.pose.position.z = 0;
     marker.color.a = 1.0;
     marker.color.r = 1.0;
     marker.color.g = 1.0;
@@ -181,6 +196,9 @@ void TrackerObjectDebugger::draw(
     text_marker.action = visualization_msgs::msg::Marker::ADD;
     text_marker.pose.position.z += 1.5;
     text_marker.scale.z = 0.8;
+    text_marker.pose.position.x = object_data_front.tracker_point.x;
+    text_marker.pose.position.y = object_data_front.tracker_point.y;
+    text_marker.pose.position.z = object_data_front.tracker_point.z + 1.0;
 
     // show the last existence probability
     std::string existence_probability_text = "P: ";
@@ -195,64 +213,77 @@ void TrackerObjectDebugger::draw(
     // loop for each object_data in the group
     // boxed to tracker positions
     // and link lines to the detected positions
+    const double line_height_offset = 1.0;
+
     visualization_msgs::msg::Marker marker_boxes;
     marker_boxes = marker;
     marker_boxes.ns = "boxes";
     marker_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST;
     marker_boxes.action = visualization_msgs::msg::Marker::ADD;
 
+    visualization_msgs::msg::Marker marker_lines;
+    marker_lines = marker;
+    marker_lines.ns = "association_lines";
+    marker_lines.type = visualization_msgs::msg::Marker::LINE_LIST;
+    marker_lines.action = visualization_msgs::msg::Marker::ADD;
+    marker_lines.scale.x = 0.2;
+    marker_lines.points.clear();
+
     for (const auto & object_data : object_data_group) {
+      // set box
+      geometry_msgs::msg::Point box_point;
+      box_point.x = object_data.tracker_point.x;
+      box_point.y = object_data.tracker_point.y;
+      box_point.z = object_data.tracker_point.z;
+      marker_boxes.points.push_back(box_point);
+      marker_boxes.scale.x = 0.2;
+      marker_boxes.scale.y = 0.2;
+      marker_boxes.scale.z = 0.2;
+
+      // set association marker, if exists
+      if (!object_data.is_associated) continue;
       // get color - by channel index
       std_msgs::msg::ColorRGBA color;
       color.a = 1.0;
       color.r = color_array[object_data.channel_id % PALETTE_SIZE][0];
       color.g = color_array[object_data.channel_id % PALETTE_SIZE][1];
       color.b = color_array[object_data.channel_id % PALETTE_SIZE][2];
+
+      // association line
+      geometry_msgs::msg::Point line_point;
+      marker_lines.color = color;
+
+      line_point.x = object_data.tracker_point.x;
+      line_point.y = object_data.tracker_point.y;
+      line_point.z = object_data.tracker_point.z + line_height_offset;
+      marker_lines.points.push_back(line_point);
+      line_point.x = object_data.detection_point.x;
+      line_point.y = object_data.detection_point.y;
+      line_point.z = object_data.tracker_point.z + line_height_offset + 2;
+      marker_lines.points.push_back(line_point);
+
+      // associated object box
+      box_point.x = object_data.detection_point.x;
+      box_point.y = object_data.detection_point.y;
+      box_point.z = object_data.detection_point.z;
       marker_boxes.color = color;
+      marker_boxes.points.push_back(box_point);
     }
+
+    // add markers
+    marker_array.markers.push_back(marker_boxes);
+    marker_array.markers.push_back(marker_lines);
   }
 
   return;
 }
 
-void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array)
+void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & marker_array) const
 {
   if (!is_initialized_) return;
 
-  // check all object data and update marker index
-  update_id_map(object_data_list_);
-
-  // update uuid_int
-  for (auto & object_data : object_data_list_) {
-    object_data.uuid_int = uuid_to_marker_id(object_data.uuid);
-  }
-
-  // sort by uuid, collect the same uuid object_data as a group, and loop for the groups
-  std::vector<std::vector<ObjectData>> object_data_groups;
-  {
-    // sort by uuid_int
-    std::sort(
-      object_data_list_.begin(), object_data_list_.end(),
-      [](const ObjectData & a, const ObjectData & b) { return a.uuid_int < b.uuid_int; });
-
-    // collect the same uuid object_data as a group
-    std::vector<ObjectData> object_data_group;
-    boost::uuids::uuid previous_uuid = object_data_list_.front().uuid;
-    for (const auto & object_data : object_data_list_) {
-      if (object_data.uuid != previous_uuid) {
-        object_data_groups.push_back(object_data_group);
-        object_data_group.clear();
-        previous_uuid = object_data.uuid;
-      }
-      object_data_group.push_back(object_data);
-    }
-
-    // fill the vector of groups
-    object_data_groups.push_back(object_data_group);
-  }
-
   // draw markers
-  draw(object_data_groups, marker_array);
+  draw(object_data_groups_, marker_array);
 
   // remove old markers
   for (const auto & previous_id : previous_ids_) {
@@ -263,13 +294,15 @@ void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & ma
     visualization_msgs::msg::Marker delete_marker;
     delete_marker.header.frame_id = frame_id_;
     delete_marker.header.stamp = message_time_;
-    delete_marker.ns = "boxes";
+    delete_marker.ns = "existence_probability";
     delete_marker.id = previous_id;
     delete_marker.action = visualization_msgs::msg::Marker::DELETE;
 
     marker_array.markers.push_back(delete_marker);
 
-    delete_marker.ns = "existence_probability";
+    delete_marker.ns = "boxes";
+    marker_array.markers.push_back(delete_marker);
+    delete_marker.ns = "association_lines";
     marker_array.markers.push_back(delete_marker);
   }
 
diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp
index 4d0d747bc2ef1..04b5baedbb369 100644
--- a/perception/multi_object_tracker/src/debugger/debugger.cpp
+++ b/perception/multi_object_tracker/src/debugger/debugger.cpp
@@ -190,7 +190,14 @@ void TrackerDebugger::collectObjectInfo(
 void TrackerDebugger::publishObjectsMarkers()
 {
   visualization_msgs::msg::MarkerArray marker_message;
+
+  // process data
+  object_debugger_.process();
+
+  // publish markers
   object_debugger_.getMessage(marker_message);
   debug_objects_markers_pub_->publish(marker_message);
+
+  // reset object data
   object_debugger_.reset();
 }

From 104dac486a52292f40bb6655cdf654c66ade2528 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 19 Apr 2024 17:07:18 +0900
Subject: [PATCH 30/67] feat: print channel names

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../debugger/debug_object.hpp                 |  6 ++
 .../debugger/debugger.hpp                     |  5 ++
 .../src/debugger/debug_object.cpp             | 84 ++++++++++++-------
 .../src/multi_object_tracker_core.cpp         |  1 +
 .../src/tracker/model/tracker_base.cpp        | 10 ++-
 5 files changed, 73 insertions(+), 33 deletions(-)

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
index 4875b6105fa9a..44137c04a3580 100644
--- 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
@@ -77,7 +77,13 @@ class TrackerObjectDebugger
   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,
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
index 8a5ebd8a61ff1..32ca7cc9effa7 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
@@ -31,6 +31,7 @@
 #include <memory>
 #include <string>
 #include <unordered_map>
+#include <vector>
 
 /**
  * @brief Debugger class for multi object tracker
@@ -89,6 +90,10 @@ class TrackerDebugger
   void checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
   // Debug object
+  void setObjectChannels(const std::vector<std::string> & channels)
+  {
+    object_debugger_.setChannelNames(channels);
+  }
   void collectObjectInfo(
     const rclcpp::Time & message_time, const std::list<std::shared_ptr<Tracker>> & list_tracker,
     const uint & channel_index,
diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp
index b7696ed7e7a44..a74885376c3c5 100644
--- a/perception/multi_object_tracker/src/debugger/debug_object.cpp
+++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp
@@ -19,6 +19,8 @@
 #include <boost/uuid/uuid.hpp>
 
 #include <functional>
+#include <iomanip>
+#include <sstream>
 #include <string>
 
 TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id)
@@ -194,60 +196,77 @@ void TrackerObjectDebugger::draw(
     text_marker.ns = "existence_probability";
     text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
     text_marker.action = visualization_msgs::msg::Marker::ADD;
-    text_marker.pose.position.z += 1.5;
-    text_marker.scale.z = 0.8;
+    text_marker.pose.position.z += 1.8;
+    text_marker.scale.z = 0.7;
     text_marker.pose.position.x = object_data_front.tracker_point.x;
     text_marker.pose.position.y = object_data_front.tracker_point.y;
     text_marker.pose.position.z = object_data_front.tracker_point.z + 1.0;
 
     // show the last existence probability
-    std::string existence_probability_text = "P: ";
-    for (const auto & existence_probability : object_data_front.existence_vector) {
-      // probability to text, two digits of percentage
-      existence_probability_text +=
-        std::to_string(static_cast<int>(existence_probability * 100)) + " ";
+    // print existence probability with channel name
+    // probability to text, two digits of percentage
+    std::string existence_probability_text = "P:";
+    for (size_t i = 0; i < object_data_front.existence_vector.size(); ++i) {
+      std::stringstream stream;
+      stream << std::fixed << std::setprecision(0) << object_data_front.existence_vector[i] * 100;
+      existence_probability_text += " " + channel_names_[i] + ":" + stream.str();
     }
+
     text_marker.text = existence_probability_text;
     marker_array.markers.push_back(text_marker);
 
     // loop for each object_data in the group
     // boxed to tracker positions
     // and link lines to the detected positions
-    const double line_height_offset = 1.0;
-
-    visualization_msgs::msg::Marker marker_boxes;
-    marker_boxes = marker;
-    marker_boxes.ns = "boxes";
-    marker_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST;
-    marker_boxes.action = visualization_msgs::msg::Marker::ADD;
+    const double height_offset = 1.0;
+
+    visualization_msgs::msg::Marker marker_track_boxes;
+    marker_track_boxes = marker;
+    marker_track_boxes.ns = "track_boxes";
+    marker_track_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST;
+    marker_track_boxes.action = visualization_msgs::msg::Marker::ADD;
+    marker_track_boxes.scale.x = 0.4;
+    marker_track_boxes.scale.y = 0.4;
+    marker_track_boxes.scale.z = 0.4;
+
+    visualization_msgs::msg::Marker marker_detect_boxes;
+    marker_detect_boxes = marker;
+    marker_detect_boxes.ns = "detect_boxes";
+    marker_detect_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST;
+    marker_detect_boxes.action = visualization_msgs::msg::Marker::ADD;
+    marker_detect_boxes.scale.x = 0.2;
+    marker_detect_boxes.scale.y = 0.2;
+    marker_detect_boxes.scale.z = 0.2;
 
     visualization_msgs::msg::Marker marker_lines;
     marker_lines = marker;
     marker_lines.ns = "association_lines";
     marker_lines.type = visualization_msgs::msg::Marker::LINE_LIST;
     marker_lines.action = visualization_msgs::msg::Marker::ADD;
-    marker_lines.scale.x = 0.2;
+    marker_lines.scale.x = 0.15;
     marker_lines.points.clear();
 
+    bool is_line_drawn = false;
+
     for (const auto & object_data : object_data_group) {
+      int channel_id = object_data.channel_id;
+
       // set box
       geometry_msgs::msg::Point box_point;
       box_point.x = object_data.tracker_point.x;
       box_point.y = object_data.tracker_point.y;
-      box_point.z = object_data.tracker_point.z;
-      marker_boxes.points.push_back(box_point);
-      marker_boxes.scale.x = 0.2;
-      marker_boxes.scale.y = 0.2;
-      marker_boxes.scale.z = 0.2;
+      box_point.z = object_data.tracker_point.z + height_offset;
+      marker_track_boxes.points.push_back(box_point);
 
       // set association marker, if exists
       if (!object_data.is_associated) continue;
+      is_line_drawn = true;
       // get color - by channel index
       std_msgs::msg::ColorRGBA color;
       color.a = 1.0;
-      color.r = color_array[object_data.channel_id % PALETTE_SIZE][0];
-      color.g = color_array[object_data.channel_id % PALETTE_SIZE][1];
-      color.b = color_array[object_data.channel_id % PALETTE_SIZE][2];
+      color.r = color_array[channel_id % PALETTE_SIZE][0];
+      color.g = color_array[channel_id % PALETTE_SIZE][1];
+      color.b = color_array[channel_id % PALETTE_SIZE][2];
 
       // association line
       geometry_msgs::msg::Point line_point;
@@ -255,24 +274,25 @@ void TrackerObjectDebugger::draw(
 
       line_point.x = object_data.tracker_point.x;
       line_point.y = object_data.tracker_point.y;
-      line_point.z = object_data.tracker_point.z + line_height_offset;
+      line_point.z = object_data.tracker_point.z + height_offset;
       marker_lines.points.push_back(line_point);
       line_point.x = object_data.detection_point.x;
       line_point.y = object_data.detection_point.y;
-      line_point.z = object_data.tracker_point.z + line_height_offset + 2;
+      line_point.z = object_data.tracker_point.z + height_offset + 1;
       marker_lines.points.push_back(line_point);
 
       // associated object box
       box_point.x = object_data.detection_point.x;
       box_point.y = object_data.detection_point.y;
-      box_point.z = object_data.detection_point.z;
-      marker_boxes.color = color;
-      marker_boxes.points.push_back(box_point);
+      box_point.z = object_data.detection_point.z + height_offset + 1;
+      marker_detect_boxes.color = color;
+      marker_detect_boxes.points.push_back(box_point);
     }
 
     // add markers
-    marker_array.markers.push_back(marker_boxes);
-    marker_array.markers.push_back(marker_lines);
+    marker_array.markers.push_back(marker_track_boxes);
+    marker_array.markers.push_back(marker_detect_boxes);
+    if (is_line_drawn) marker_array.markers.push_back(marker_lines);
   }
 
   return;
@@ -300,7 +320,9 @@ void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & ma
 
     marker_array.markers.push_back(delete_marker);
 
-    delete_marker.ns = "boxes";
+    delete_marker.ns = "track_boxes";
+    marker_array.markers.push_back(delete_marker);
+    delete_marker.ns = "detect_boxes";
     marker_array.markers.push_back(delete_marker);
     delete_marker.ns = "association_lines";
     marker_array.markers.push_back(delete_marker);
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index a80f12442f75e..d9da1f78f963b 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -166,6 +166,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
 
   // Debugger
   debugger_ = std::make_unique<TrackerDebugger>(*this, world_frame_id_);
+  debugger_->setObjectChannels(input_names_short);
   published_time_publisher_ = std::make_unique<tier4_autoware_utils::PublishedTimePublisher>(this);
 }
 
diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
index 74912655ea0d6..7c73ebae131d2 100644
--- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
+++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
@@ -43,7 +43,7 @@ Tracker::Tracker(
 void Tracker::initializeExistenceProbabilities(
   const uint & channel_index, const float & existence_probability)
 {
-  existence_probabilities_[channel_index] = existence_probability;
+  existence_probabilities_[channel_index] = 0.8 + 0.2 * existence_probability;
   total_existence_probability_ = existence_probability;
 }
 
@@ -61,7 +61,13 @@ bool Tracker::updateWithMeasurement(
     // existence probability on each channel
     const double delta_time = (measurement_time - last_update_with_measurement_time_).seconds();
     const double decay_rate = 5.0 / 10.0;
-    existence_probabilities_[channel_index] = existence_probability_from_object;
+
+    const float gain = 0.8;
+    const float probability_detected = 0.8;
+    // existence_probabilities_[channel_index] = existence_probability_from_object;
+    existence_probabilities_[channel_index] =
+      gain * probability_detected + (1 - gain) * existence_probabilities_[channel_index];
+
     for (size_t i = 0; i < existence_probabilities_.size(); ++i) {
       if (i == channel_index) {
         continue;

From 8d4408b817f28c2236211723780889f7909d4283 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 19 Apr 2024 17:39:59 +0900
Subject: [PATCH 31/67] feat: association lines are color-coded

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../src/debugger/debug_object.cpp             | 115 +++++++++++-------
 1 file changed, 73 insertions(+), 42 deletions(-)

diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp
index a74885376c3c5..8593afd77a30d 100644
--- a/perception/multi_object_tracker/src/debugger/debug_object.cpp
+++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp
@@ -229,24 +229,41 @@ void TrackerObjectDebugger::draw(
     marker_track_boxes.scale.y = 0.4;
     marker_track_boxes.scale.z = 0.4;
 
-    visualization_msgs::msg::Marker marker_detect_boxes;
-    marker_detect_boxes = marker;
-    marker_detect_boxes.ns = "detect_boxes";
-    marker_detect_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST;
-    marker_detect_boxes.action = visualization_msgs::msg::Marker::ADD;
-    marker_detect_boxes.scale.x = 0.2;
-    marker_detect_boxes.scale.y = 0.2;
-    marker_detect_boxes.scale.z = 0.2;
-
-    visualization_msgs::msg::Marker marker_lines;
-    marker_lines = marker;
-    marker_lines.ns = "association_lines";
-    marker_lines.type = visualization_msgs::msg::Marker::LINE_LIST;
-    marker_lines.action = visualization_msgs::msg::Marker::ADD;
-    marker_lines.scale.x = 0.15;
-    marker_lines.points.clear();
-
-    bool is_line_drawn = false;
+    // make detected object markers per channel
+    std::vector<visualization_msgs::msg::Marker> marker_detect_boxes_per_channel;
+    std::vector<visualization_msgs::msg::Marker> marker_detect_lines_per_channel;
+
+    for (size_t idx = 0; idx < channel_names_.size(); idx++) {
+      // get color - by channel index
+      std_msgs::msg::ColorRGBA color;
+      color.a = 1.0;
+      color.r = color_array[idx % PALETTE_SIZE][0];
+      color.g = color_array[idx % PALETTE_SIZE][1];
+      color.b = color_array[idx % PALETTE_SIZE][2];
+
+      visualization_msgs::msg::Marker marker_detect_boxes;
+      marker_detect_boxes = marker;
+      marker_detect_boxes.ns = "detect_boxes_" + channel_names_[idx];
+      marker_detect_boxes.type = visualization_msgs::msg::Marker::CUBE_LIST;
+      marker_detect_boxes.action = visualization_msgs::msg::Marker::ADD;
+      marker_detect_boxes.scale.x = 0.2;
+      marker_detect_boxes.scale.y = 0.2;
+      marker_detect_boxes.scale.z = 0.2;
+      marker_detect_boxes.color = color;
+      marker_detect_boxes_per_channel.push_back(marker_detect_boxes);
+
+      visualization_msgs::msg::Marker marker_lines;
+      marker_lines = marker;
+      marker_lines.ns = "association_lines_" + channel_names_[idx];
+      marker_lines.type = visualization_msgs::msg::Marker::LINE_LIST;
+      marker_lines.action = visualization_msgs::msg::Marker::ADD;
+      marker_lines.scale.x = 0.15;
+      marker_lines.points.clear();
+      marker_lines.color = color;
+      marker_detect_lines_per_channel.push_back(marker_lines);
+    }
+
+    bool is_any_associated = false;
 
     for (const auto & object_data : object_data_group) {
       int channel_id = object_data.channel_id;
@@ -260,39 +277,51 @@ void TrackerObjectDebugger::draw(
 
       // set association marker, if exists
       if (!object_data.is_associated) continue;
-      is_line_drawn = true;
-      // get color - by channel index
-      std_msgs::msg::ColorRGBA color;
-      color.a = 1.0;
-      color.r = color_array[channel_id % PALETTE_SIZE][0];
-      color.g = color_array[channel_id % PALETTE_SIZE][1];
-      color.b = color_array[channel_id % PALETTE_SIZE][2];
+      is_any_associated = true;
+
+      // associated object box
+      visualization_msgs::msg::Marker & marker_detect_boxes =
+        marker_detect_boxes_per_channel.at(channel_id);
+      box_point.x = object_data.detection_point.x;
+      box_point.y = object_data.detection_point.y;
+      box_point.z = object_data.detection_point.z + height_offset + 1;
+      marker_detect_boxes.points.push_back(box_point);
 
       // association line
+      visualization_msgs::msg::Marker & marker_lines =
+        marker_detect_lines_per_channel.at(channel_id);
       geometry_msgs::msg::Point line_point;
-      marker_lines.color = color;
-
       line_point.x = object_data.tracker_point.x;
       line_point.y = object_data.tracker_point.y;
       line_point.z = object_data.tracker_point.z + height_offset;
       marker_lines.points.push_back(line_point);
       line_point.x = object_data.detection_point.x;
       line_point.y = object_data.detection_point.y;
-      line_point.z = object_data.tracker_point.z + height_offset + 1;
+      line_point.z = object_data.detection_point.z + height_offset + 1;
       marker_lines.points.push_back(line_point);
-
-      // associated object box
-      box_point.x = object_data.detection_point.x;
-      box_point.y = object_data.detection_point.y;
-      box_point.z = object_data.detection_point.z + height_offset + 1;
-      marker_detect_boxes.color = color;
-      marker_detect_boxes.points.push_back(box_point);
     }
 
     // add markers
     marker_array.markers.push_back(marker_track_boxes);
-    marker_array.markers.push_back(marker_detect_boxes);
-    if (is_line_drawn) marker_array.markers.push_back(marker_lines);
+    if (is_any_associated) {
+      for (size_t i = 0; i < channel_names_.size(); i++) {
+        if (marker_detect_boxes_per_channel.at(i).points.empty()) continue;
+        marker_array.markers.push_back(marker_detect_boxes_per_channel.at(i));
+      }
+      for (size_t i = 0; i < channel_names_.size(); i++) {
+        if (marker_detect_lines_per_channel.at(i).points.empty()) continue;
+        marker_array.markers.push_back(marker_detect_lines_per_channel.at(i));
+      }
+    } else {
+      for (size_t i = 0; i < channel_names_.size(); i++) {
+        marker_detect_boxes_per_channel.at(i).action = visualization_msgs::msg::Marker::DELETE;
+        marker_array.markers.push_back(marker_detect_boxes_per_channel.at(i));
+      }
+      for (size_t i = 0; i < channel_names_.size(); i++) {
+        marker_detect_lines_per_channel.at(i).action = visualization_msgs::msg::Marker::DELETE;
+        marker_array.markers.push_back(marker_detect_lines_per_channel.at(i));
+      }
+    }
   }
 
   return;
@@ -317,15 +346,17 @@ void TrackerObjectDebugger::getMessage(visualization_msgs::msg::MarkerArray & ma
     delete_marker.ns = "existence_probability";
     delete_marker.id = previous_id;
     delete_marker.action = visualization_msgs::msg::Marker::DELETE;
-
     marker_array.markers.push_back(delete_marker);
 
     delete_marker.ns = "track_boxes";
     marker_array.markers.push_back(delete_marker);
-    delete_marker.ns = "detect_boxes";
-    marker_array.markers.push_back(delete_marker);
-    delete_marker.ns = "association_lines";
-    marker_array.markers.push_back(delete_marker);
+
+    for (size_t idx = 0; idx < channel_names_.size(); idx++) {
+      delete_marker.ns = "detect_boxes_" + channel_names_[idx];
+      marker_array.markers.push_back(delete_marker);
+      delete_marker.ns = "association_lines_" + channel_names_[idx];
+      marker_array.markers.push_back(delete_marker);
+    }
   }
 
   return;

From 30aed23da45e900041931da01879e36eb3fd0cad Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 22 Apr 2024 12:04:10 +0900
Subject: [PATCH 32/67] fix: association debug marker bugfix

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../src/multi_object_tracker_core.cpp                 | 11 ++++++-----
 .../src/processor/input_manager.cpp                   | 10 ----------
 2 files changed, 6 insertions(+), 15 deletions(-)

diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index d9da1f78f963b..a6c54e3a2c3c2 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -291,6 +291,12 @@ void MultiObjectTracker::runProcess(
     data_association_->assign(score_matrix, direct_assignment, reverse_assignment);
   }
 
+  // Collect debug information - tracker list, existence probabilities, association result
+  // TODO(technolojin): add option to enable/disable debug information
+  debugger_->collectObjectInfo(
+    measurement_time, processor_->getListTracker(), channel_index, transformed_objects,
+    direct_assignment, reverse_assignment);
+
   /* tracker update */
   processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index);
 
@@ -300,11 +306,6 @@ void MultiObjectTracker::runProcess(
   /* spawn new tracker */
   processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index);
 
-  // Collect debug information - tracker list, existence probabilities, association result
-  // TODO(technolojin): add option to enable/disable debug information
-  debugger_->collectObjectInfo(
-    measurement_time, processor_->getListTracker(), channel_index, transformed_objects,
-    direct_assignment, reverse_assignment);
 }
 
 void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time)
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 5120f6632ead4..5ebbacad90ba3 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -58,19 +58,12 @@ bool InputStream::getTimestamps(
 void InputStream::onMessage(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)
 {
-  // // debug message
-  // RCLCPP_INFO(
-  //   node_.get_logger(), "InputStream::onMessage Received %s message from %s at %d.%d",
-  //   long_name_.c_str(), input_topic_.c_str(), msg->header.stamp.sec, msg->header.stamp.nanosec);
-
   const DetectedObjects objects = *msg;
   objects_que_.push_back(objects);
   if (objects_que_.size() > que_size_) {
     objects_que_.pop_front();
   }
 
-  // RCLCPP_INFO(node_.get_logger(), "InputStream::onMessage Que size: %zu", objects_que_.size());
-
   // Filter parameters
   constexpr double gain = 0.05;
   const auto now = node_.now();
@@ -125,9 +118,6 @@ void InputStream::getObjectsOlderThan(
       objects_que_.pop_front();
     }
   }
-  // RCLCPP_INFO(
-  //   node_.get_logger(), "InputStream::getObjectsOlderThan %s gives %zu objects",
-  //   long_name_.c_str(), objects.size());
 }
 
 InputManager::InputManager(rclcpp::Node & node) : node_(node)

From 78014b99376075b39ab540827728d87ea0829a04 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Mon, 22 Apr 2024 03:06:06 +0000
Subject: [PATCH 33/67] style(pre-commit): autofix

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker/src/multi_object_tracker_core.cpp       | 1 -
 1 file changed, 1 deletion(-)

diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index a6c54e3a2c3c2..ebc06388d0ad7 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -305,7 +305,6 @@ void MultiObjectTracker::runProcess(
 
   /* spawn new tracker */
   processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index);
-
 }
 
 void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time)

From d036f32e459bb231f2b9cd4084feeceecf8d997f Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 22 Apr 2024 18:04:20 +0900
Subject: [PATCH 34/67] feat: add option for debug marker

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_node.param.yaml      |  3 +-
 .../debugger/debugger.hpp                     |  1 +
 .../src/debugger/debug_object.cpp             |  9 ++--
 .../src/debugger/debugger.cpp                 | 12 +++--
 .../src/multi_object_tracker_core.cpp         | 50 ++++++-------------
 5 files changed, 33 insertions(+), 42 deletions(-)

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
index 945cf44113a06..7603d82a75a12 100644
--- 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,9 +17,10 @@
     # 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]
 
     input_topic_names: ["/perception/object_recognition/detection/objects"]
-    input_names_long: ["merged_objects"]
+    input_names_long: ["detected_objects"]
     input_names_short: ["all"]
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
index 32ca7cc9effa7..8edfc102b56db 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
@@ -47,6 +47,7 @@ class TrackerDebugger
   {
     bool publish_processing_time;
     bool publish_tentative_objects;
+    bool publish_debug_markers;
     double diagnostics_warn_delay;
     double diagnostics_error_delay;
   } debug_settings_;
diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp
index 8593afd77a30d..3f8cb3badc9b1 100644
--- a/perception/multi_object_tracker/src/debugger/debug_object.cpp
+++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp
@@ -197,19 +197,20 @@ void TrackerObjectDebugger::draw(
     text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
     text_marker.action = visualization_msgs::msg::Marker::ADD;
     text_marker.pose.position.z += 1.8;
-    text_marker.scale.z = 0.7;
+    text_marker.scale.z = 0.5;
     text_marker.pose.position.x = object_data_front.tracker_point.x;
     text_marker.pose.position.y = object_data_front.tracker_point.y;
-    text_marker.pose.position.z = object_data_front.tracker_point.z + 1.0;
+    text_marker.pose.position.z = object_data_front.tracker_point.z + 2.0;
 
     // show the last existence probability
     // print existence probability with channel name
     // probability to text, two digits of percentage
-    std::string existence_probability_text = "P:";
+    std::string existence_probability_text = "";
+    existence_probability_text += std::to_string(uuid_int);
     for (size_t i = 0; i < object_data_front.existence_vector.size(); ++i) {
       std::stringstream stream;
       stream << std::fixed << std::setprecision(0) << object_data_front.existence_vector[i] * 100;
-      existence_probability_text += " " + channel_names_[i] + ":" + stream.str();
+      existence_probability_text += ":" + channel_names_[i] + stream.str();
     }
 
     text_marker.text = existence_probability_text;
diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp
index 04b5baedbb369..0334eca58de3a 100644
--- a/perception/multi_object_tracker/src/debugger/debugger.cpp
+++ b/perception/multi_object_tracker/src/debugger/debugger.cpp
@@ -33,8 +33,10 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_
         "debug/tentative_objects", rclcpp::QoS{1});
   }
 
-  debug_objects_markers_pub_ = node_.create_publisher<visualization_msgs::msg::MarkerArray>(
-    "multi_object_tracker/debug/objects_markers", rclcpp::QoS{1});
+  if (debug_settings_.publish_debug_markers) {
+    debug_objects_markers_pub_ = node_.create_publisher<visualization_msgs::msg::MarkerArray>(
+      "multi_object_tracker/debug/objects_markers", rclcpp::QoS{1});
+  }
 
   // initialize timestamps
   const rclcpp::Time now = node_.now();
@@ -55,6 +57,7 @@ void TrackerDebugger::loadParameters()
       node_.declare_parameter<bool>("publish_processing_time");
     debug_settings_.publish_tentative_objects =
       node_.declare_parameter<bool>("publish_tentative_objects");
+    debug_settings_.publish_debug_markers = node_.declare_parameter<bool>("publish_debug_markers");
     debug_settings_.diagnostics_warn_delay =
       node_.declare_parameter<double>("diagnostics_warn_delay");
     debug_settings_.diagnostics_error_delay =
@@ -63,6 +66,7 @@ void TrackerDebugger::loadParameters()
     RCLCPP_WARN(node_.get_logger(), "Failed to declare parameter: %s", e.what());
     debug_settings_.publish_processing_time = false;
     debug_settings_.publish_tentative_objects = false;
+    debug_settings_.publish_debug_markers = false;
     debug_settings_.diagnostics_warn_delay = 0.5;
     debug_settings_.diagnostics_error_delay = 1.0;
   }
@@ -181,6 +185,7 @@ void TrackerDebugger::collectObjectInfo(
   const std::unordered_map<int, int> & direct_assignment,
   const std::unordered_map<int, int> & reverse_assignment)
 {
+  if (!debug_settings_.publish_debug_markers) return;
   object_debugger_.collect(
     message_time, list_tracker, channel_index, detected_objects, direct_assignment,
     reverse_assignment);
@@ -189,8 +194,9 @@ void TrackerDebugger::collectObjectInfo(
 // ObjectDebugger
 void TrackerDebugger::publishObjectsMarkers()
 {
-  visualization_msgs::msg::MarkerArray marker_message;
+  if (!debug_settings_.publish_debug_markers) return;
 
+  visualization_msgs::msg::MarkerArray marker_message;
   // process data
   object_debugger_.process();
 
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index ebc06388d0ad7..9fbad2bf6bb18 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -92,13 +92,21 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
 
   // ROS interface - Subscribers
   if (input_topic_names.empty()) {
-    RCLCPP_ERROR(get_logger(), "Need a 'input_topic_names' parameter to be set before continuing");
+    RCLCPP_ERROR(this->get_logger(), "No input topics are specified.");
     return;
   }
+  if (
+    input_names_long.size() != input_topic_names.size() ||
+    input_names_short.size() != input_topic_names.size()) {
+    RCLCPP_ERROR(
+      this->get_logger(),
+      "The number of input topic names, long names, and short names must be the same.");
+    return;
+  }
+
   input_channel_size_ = input_topic_names.size();
   input_channels_.resize(input_channel_size_);
-  assert(input_names_long.size() == input_channel_size_);
-  assert(input_names_short.size() == input_channel_size_);
+
   for (size_t i = 0; i < input_channel_size_; i++) {
     input_channels_[i].index = i;
     input_channels_[i].input_topic = input_topic_names[i];
@@ -229,31 +237,6 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
   }
   // process end
   debugger_->endMeasurementTime(this->now());
-
-  /* DEBUG */
-  const rclcpp::Time latest_time(objects_list.back().second.header.stamp);
-  RCLCPP_INFO(
-    this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f",
-    (current_time - latest_time).seconds(), (current_time - oldest_time).seconds());
-
-  // count objects on each channel
-  std::vector<int> object_counts;
-  object_counts.resize(input_channel_size_);
-  // initialize to zero
-  for (size_t i = 0; i < input_channel_size_; i++) {
-    object_counts[i] = 0;
-  }
-  for (const auto & objects_data : objects_list) {
-    object_counts[objects_data.first] += 1;
-  }
-  // print result
-  std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: ";
-  for (size_t i = 0; i < input_channel_size_; i++) {
-    object_counts_str +=
-      input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + "  ";
-  }
-  RCLCPP_INFO(this->get_logger(), object_counts_str.c_str());
-  /* DEBUG END */
 }
 
 void MultiObjectTracker::runProcess(
@@ -289,13 +272,12 @@ void MultiObjectTracker::runProcess(
     Eigen::MatrixXd score_matrix = data_association_->calcScoreMatrix(
       detected_objects, list_tracker);  // row : tracker, col : measurement
     data_association_->assign(score_matrix, direct_assignment, reverse_assignment);
-  }
 
-  // Collect debug information - tracker list, existence probabilities, association result
-  // TODO(technolojin): add option to enable/disable debug information
-  debugger_->collectObjectInfo(
-    measurement_time, processor_->getListTracker(), channel_index, transformed_objects,
-    direct_assignment, reverse_assignment);
+    // Collect debug information - tracker list, existence probabilities, association results
+    debugger_->collectObjectInfo(
+      measurement_time, processor_->getListTracker(), channel_index, transformed_objects,
+      direct_assignment, reverse_assignment);
+  }
 
   /* tracker update */
   processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index);

From 8d6009d0566ad80912c01db8136996e8fe786d2b Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Tue, 23 Apr 2024 18:17:02 +0900
Subject: [PATCH 35/67] feat: skip time statistics update in case of outlier

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../src/multi_object_tracker_core.cpp         | 38 +++++++++++++++++++
 .../src/processor/input_manager.cpp           | 23 ++++++++---
 2 files changed, 56 insertions(+), 5 deletions(-)

diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 9fbad2bf6bb18..f423999e803b5 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -237,6 +237,44 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
   }
   // process end
   debugger_->endMeasurementTime(this->now());
+
+  /* DEBUG */
+  const rclcpp::Time latest_time(objects_list.back().second.header.stamp);
+  RCLCPP_INFO(
+    this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f",
+    (current_time - latest_time).seconds(), (current_time - oldest_time).seconds());
+
+  // count objects on each channel
+  std::vector<int> object_counts;
+  object_counts.resize(input_channel_size_);
+  // initialize to zero
+  for (size_t i = 0; i < input_channel_size_; i++) {
+    object_counts[i] = 0;
+  }
+  for (const auto & objects_data : objects_list) {
+    object_counts[objects_data.first] += 1;
+  }
+  // print result
+  std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: ";
+  for (size_t i = 0; i < input_channel_size_; i++) {
+    object_counts_str +=
+      input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + "  ";
+  }
+  RCLCPP_INFO(this->get_logger(), object_counts_str.c_str());
+
+  std::string object_info_str = "";
+  object_info_str += "MultiObjectTracker::onMessage Objects channel and time: \n";
+  for (const auto & objects_data : objects_list) {
+    const auto & objects = objects_data.second;
+    const auto & channel_index = objects_data.first;
+    const auto & time = rclcpp::Time(objects.header.stamp);
+    // print object information
+    object_info_str += input_channels_[channel_index].long_name + " " +
+                       std::to_string((current_time - time).seconds()) + " \n";
+  }
+  RCLCPP_INFO(this->get_logger(), object_info_str.c_str());
+
+  /* DEBUG END */
 }
 
 void MultiObjectTracker::runProcess(
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 5ebbacad90ba3..67cbcbb71751a 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -70,19 +70,30 @@ void InputStream::onMessage(
 
   // Calculate interval, Update interval statistics
   if (is_time_initialized_) {
+    bool is_interval_regular = true;
     const double interval = (now - latest_message_time_).seconds();
-    interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval;
-    const double interval_delta = interval - interval_mean_;
-    interval_var_ = (1.0 - gain) * interval_var_ + gain * interval_delta * interval_delta;
+    // check if it is outlier
+    if (interval > 1.5 * interval_mean_ || interval < 0.5 * interval_mean_) {
+      // no update for the time statistics
+      is_interval_regular = false;
+    }
+
+    if (is_interval_regular) {
+      interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval;
+      const double interval_delta = interval - interval_mean_;
+      interval_var_ = (1.0 - gain) * interval_var_ + gain * interval_delta * interval_delta;
+    }
   }
 
   // Update time
   latest_message_time_ = now;
-  latest_measurement_time_ = objects.header.stamp;
+  rclcpp::Time objects_time(objects.header.stamp);
+  latest_measurement_time_ =
+    latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_;
   if (!is_time_initialized_) is_time_initialized_ = true;
 
   // Calculate latency
-  const double latency = (latest_message_time_ - latest_measurement_time_).seconds();
+  const double latency = (latest_message_time_ - objects_time).seconds();
 
   // Update latency statistics
   latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency;
@@ -184,6 +195,8 @@ void InputManager::getObjectTimeInterval(
       }
       double latency_message = (now - latest_message_time).seconds();
       double latency_measurement = (now - latest_measurement_time).seconds();
+
+      // print info
       RCLCPP_INFO(
         node_.get_logger(),
         "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: "

From 90d287ce7080dab63b5c0c873d0927ef680e879a Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Wed, 24 Apr 2024 19:27:36 +0900
Subject: [PATCH 36/67] feat: auto-tune latency band

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker_node.param.yaml      |  1 +
 .../processor/input_manager.hpp               | 10 +++-
 .../src/multi_object_tracker_core.cpp         |  7 ++-
 .../src/processor/input_manager.cpp           | 51 ++++++++++++-------
 4 files changed, 48 insertions(+), 21 deletions(-)

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
index 7603d82a75a12..7e6f9cdbfbe86 100644
--- a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml
+++ b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml
@@ -24,3 +24,4 @@
     input_topic_names: ["/perception/object_recognition/detection/objects"]
     input_names_long: ["detected_objects"]
     input_names_short: ["all"]
+    input_priority: [0]
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 725503c880915..701a01a6dc0c1 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -33,12 +33,12 @@ using ObjectsList = std::vector<std::pair<uint, DetectedObjects>>;
 
 struct InputChannel
 {
-  size_t index;             // index of the input channel
   std::string input_topic;  // topic name of the detection, e.g. "/detection/lidar"
   std::string long_name = "Detected Object";  // full name of the detection
   std::string short_name = "DET";             // abbreviation of the name
   double expected_rate = 10.0;                // [Hz]
   double expected_latency = 0.2;              // [s]
+  int priority = 0;                           // priority of the input channel, higher is better
 };
 
 class InputStream
@@ -56,6 +56,8 @@ class InputStream
 
   void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
 
+  uint getIndex() const { return index_; }
+  int getPriority() const { return priority_; }
   void getObjectsOlderThan(
     const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
     ObjectsList & objects_list);
@@ -64,6 +66,7 @@ class InputStream
     long_name = long_name_;
     short_name = short_name_;
   }
+  std::string getLongName() const { return long_name_; }
   size_t getObjectsCount() const { return objects_que_.size(); }
   void getTimeStatistics(
     double & latency_mean, double & latency_var, double & interval_mean,
@@ -85,6 +88,8 @@ class InputStream
   std::string long_name_;
   std::string short_name_;
 
+  int priority_{0};
+
   size_t que_size_{30};
   std::deque<DetectedObjects> objects_que_;
 
@@ -126,6 +131,9 @@ class InputManager
   std::vector<std::shared_ptr<InputStream>> input_streams_;
 
   std::function<void()> func_trigger_;
+  uint target_stream_idx_{0};
+  double target_latency_{0.2};
+  double target_latency_band_{0.14};
 };
 
 }  // namespace multi_object_tracker
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index f423999e803b5..a387a96244794 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -83,9 +83,11 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   declare_parameter("input_topic_names", std::vector<std::string>());
   declare_parameter("input_names_long", std::vector<std::string>());
   declare_parameter("input_names_short", std::vector<std::string>());
+  declare_parameter("input_priority", std::vector<int64_t>());
   std::vector<std::string> input_topic_names = get_parameter("input_topic_names").as_string_array();
   std::vector<std::string> input_names_long = get_parameter("input_names_long").as_string_array();
   std::vector<std::string> input_names_short = get_parameter("input_names_short").as_string_array();
+  std::vector<int64_t> input_priority = get_parameter("input_priority").as_integer_array();
 
   // ROS interface - Publisher
   tracked_objects_pub_ = create_publisher<TrackedObjects>("output", rclcpp::QoS{1});
@@ -97,7 +99,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   }
   if (
     input_names_long.size() != input_topic_names.size() ||
-    input_names_short.size() != input_topic_names.size()) {
+    input_names_short.size() != input_topic_names.size() ||
+    input_priority.size() != input_topic_names.size()) {
     RCLCPP_ERROR(
       this->get_logger(),
       "The number of input topic names, long names, and short names must be the same.");
@@ -108,10 +111,10 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   input_channels_.resize(input_channel_size_);
 
   for (size_t i = 0; i < input_channel_size_; i++) {
-    input_channels_[i].index = i;
     input_channels_[i].input_topic = input_topic_names[i];
     input_channels_[i].long_name = input_names_long[i];
     input_channels_[i].short_name = input_names_short[i];
+    input_channels_[i].priority = static_cast<int>(input_priority[i]);
   }
 
   // Initialize input manager
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 67cbcbb71751a..39a6f5d253871 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -30,6 +30,8 @@ void InputStream::init(const InputChannel & input_channel)
   long_name_ = input_channel.long_name;
   short_name_ = input_channel.short_name;
 
+  priority_ = input_channel.priority;
+
   // Initialize queue
   objects_que_.clear();
 
@@ -170,11 +172,8 @@ void InputManager::init(const std::vector<InputChannel> & input_channels)
 
 void InputManager::onTrigger(const uint & index) const
 {
-  // input stream index of 0 is the target(main) input stream
-  const uint target_idx = 0;
-
-  // when the main stream triggers, call the trigger function
-  if (index == target_idx && func_trigger_) {
+  // when the target stream triggers, call the trigger function
+  if (index == target_stream_idx_ && func_trigger_) {
     func_trigger_();
   }
 }
@@ -182,6 +181,20 @@ void InputManager::onTrigger(const uint & index) const
 void InputManager::getObjectTimeInterval(
   const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time)
 {
+  object_latest_time = now - rclcpp::Duration::from_seconds(target_latency_);
+  object_oldest_time = now - rclcpp::Duration::from_seconds(target_latency_ + target_latency_band_);
+
+  // if the object_oldest_time is older than the latest object time, set it to the latest object
+  // time
+  object_oldest_time =
+    object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_;
+
+  // OPTIMIZATION: Get the stream with the maximum latency
+  double max_latency_mean = 0.0;
+  uint stream_selected_idx = 0;
+  double selected_idx_latency_std = 0.0;
+  double selected_idx_interval = 0.0;
+
   {
     // ANALYSIS: Get the streams statistics
     std::string long_name, short_name;
@@ -195,6 +208,12 @@ void InputManager::getObjectTimeInterval(
       }
       double latency_message = (now - latest_message_time).seconds();
       double latency_measurement = (now - latest_measurement_time).seconds();
+      if (latency_mean > max_latency_mean && input_stream->getPriority() > 0) {
+        max_latency_mean = latency_mean;
+        selected_idx_latency_std = std::sqrt(latency_var);
+        stream_selected_idx = input_stream->getIndex();
+        selected_idx_interval = interval_mean;
+      }
 
       // print info
       RCLCPP_INFO(
@@ -206,19 +225,15 @@ void InputManager::getObjectTimeInterval(
     }
   }
 
-  // Get proper latency
-  constexpr double target_latency = 0.18;  // [s], measurement to tracking latency
-                                           // process latency of a main detection + margin
-  constexpr double acceptable_latency =
-    0.32;  // [s], acceptable band from the target latency, larger than the target latency
-
-  object_latest_time = now - rclcpp::Duration::from_seconds(target_latency);
-  object_oldest_time = now - rclcpp::Duration::from_seconds(acceptable_latency);
-
-  // if the object_oldest_time is older than the latest object time, set it to the latest object
-  // time
-  object_oldest_time =
-    object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_;
+  // Set the target stream index, which has the maximum latency
+  // trigger will be called next time
+  target_stream_idx_ = stream_selected_idx;
+  target_latency_ = max_latency_mean - selected_idx_latency_std;
+  target_latency_band_ = 2 * selected_idx_latency_std + selected_idx_interval;
+  RCLCPP_INFO(
+    node_.get_logger(), "InputManager::getObjects Target stream: %s, target latency: %f, band: %f",
+    input_streams_.at(target_stream_idx_)->getLongName().c_str(), target_latency_,
+    target_latency_band_);
 }
 
 bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_list)

From ca91b5bfbd18c20436ce9bd5642a793c7d817c23 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 26 Apr 2024 11:05:39 +0900
Subject: [PATCH 37/67] feat: pre-defined channels, select on launcher

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../config/input_channels.param.yaml          | 38 +++++++++++++++
 .../multi_object_tracker_node.param.yaml      |  5 --
 .../launch/multi_object_tracker.launch.xml    |  6 ++-
 .../src/multi_object_tracker_core.cpp         | 46 +++++++++++--------
 4 files changed, 69 insertions(+), 26 deletions(-)
 create mode 100644 perception/multi_object_tracker/config/input_channels.param.yaml

diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml
new file mode 100644
index 0000000000000..09444dd30ec94
--- /dev/null
+++ b/perception/multi_object_tracker/config/input_channels.param.yaml
@@ -0,0 +1,38 @@
+/**:
+  ros__parameters:
+    input_channels:
+      detected_objects:
+        topic: "/perception/object_recognition/detection/objects"
+        name: "detected_objects"
+        name_short: "all"
+        expected_frequency: 10.0
+      centerpoint:
+        topic: "/perception/object_recognition/detection/centerpoint/objects"
+        name: "centerpoint"
+        name_short: "Lcp"
+        expected_frequency: 10.0
+      centerpoint_validated:
+        topic: "/perception/object_recognition/detection/centerpoint/validation/objects"
+        name: "centerpoint"
+        name_short: "Lcp"
+        expected_frequency: 10.0
+      camera_lidar_fusion:
+        topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects"
+        name: "camera_lidar_fusion"
+        name_short: "CLf"
+        expected_frequency: 10.0
+      camera_lidar_fusion_filtered:
+        topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects"
+        name: "camera_lidar_fusion"
+        name_short: "CLf"
+        expected_frequency: 10.0
+      detection_by_tracker:
+        topic: "/perception/object_recognition/detection/detection_by_tracker/objects"
+        name: "detection_by_tracker"
+        name_short: "Ld"
+        expected_frequency: 10.0
+      radar_far:
+        topic: "/perception/object_recognition/detection/radar/far_objects"
+        name: "radar_far"
+        name_short: "Rf"
+        expected_frequency: 30.0
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
index 7e6f9cdbfbe86..09621a40c499f 100644
--- a/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml
+++ b/perception/multi_object_tracker/config/multi_object_tracker_node.param.yaml
@@ -20,8 +20,3 @@
     publish_debug_markers: false
     diagnostics_warn_delay: 0.5      # [sec]
     diagnostics_error_delay: 1.0     # [sec]
-
-    input_topic_names: ["/perception/object_recognition/detection/objects"]
-    input_names_long: ["detected_objects"]
-    input_names_short: ["all"]
-    input_priority: [0]
diff --git a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml
index dcd080b851c20..526ce23efd33b 100644
--- a/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml
+++ b/perception/multi_object_tracker/launch/multi_object_tracker.launch.xml
@@ -1,14 +1,16 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-  <arg name="input" default="/perception/object_recognition/detection/objects"/>
+  <arg name="selected_input_channels" default="['detected_objects']"/>
   <arg name="output" default="objects"/>
   <arg name="tracker_setting_path" default="$(find-pkg-share multi_object_tracker)/config/multi_object_tracker_node.param.yaml"/>
   <arg name="data_association_matrix_path" default="$(find-pkg-share multi_object_tracker)/config/data_association_matrix.param.yaml"/>
+  <arg name="input_channels_path" default="$(find-pkg-share multi_object_tracker)/config/input_channels.param.yaml"/>
 
   <node pkg="multi_object_tracker" exec="multi_object_tracker" name="multi_object_tracker" output="screen">
-    <remap from="input" to="$(var input)"/>
+    <param name="selected_input_channels" value="$(var selected_input_channels)"/>
     <remap from="output" to="$(var output)"/>
     <param from="$(var tracker_setting_path)"/>
     <param from="$(var data_association_matrix_path)"/>
+    <param from="$(var input_channels_path)"/>
   </node>
 </launch>
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index a387a96244794..7af2922dfd334 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -80,31 +80,38 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   world_frame_id_ = declare_parameter<std::string>("world_frame_id");
   bool enable_delay_compensation{declare_parameter<bool>("enable_delay_compensation")};
 
-  declare_parameter("input_topic_names", std::vector<std::string>());
-  declare_parameter("input_names_long", std::vector<std::string>());
-  declare_parameter("input_names_short", std::vector<std::string>());
-  declare_parameter("input_priority", std::vector<int64_t>());
-  std::vector<std::string> input_topic_names = get_parameter("input_topic_names").as_string_array();
-  std::vector<std::string> input_names_long = get_parameter("input_names_long").as_string_array();
-  std::vector<std::string> input_names_short = get_parameter("input_names_short").as_string_array();
-  std::vector<int64_t> input_priority = get_parameter("input_priority").as_integer_array();
+  declare_parameter("selected_input_channels", std::vector<std::string>());
+  std::vector<std::string> selected_input_channels =
+    get_parameter("selected_input_channels").as_string_array();
 
   // ROS interface - Publisher
   tracked_objects_pub_ = create_publisher<TrackedObjects>("output", rclcpp::QoS{1});
 
-  // ROS interface - Subscribers
-  if (input_topic_names.empty()) {
+  // ROS interface - Input channels
+  // Get input channels
+  std::vector<std::string> input_topic_names;
+  std::vector<std::string> input_names_long;
+  std::vector<std::string> input_names_short;
+  std::vector<double> input_expected_rates;
+
+  if (selected_input_channels.empty()) {
     RCLCPP_ERROR(this->get_logger(), "No input topics are specified.");
     return;
   }
-  if (
-    input_names_long.size() != input_topic_names.size() ||
-    input_names_short.size() != input_topic_names.size() ||
-    input_priority.size() != input_topic_names.size()) {
-    RCLCPP_ERROR(
-      this->get_logger(),
-      "The number of input topic names, long names, and short names must be the same.");
-    return;
+
+  for (const auto & selected_input_channel : selected_input_channels) {
+    const std::string input_topic_name =
+      declare_parameter<std::string>("input_channels." + selected_input_channel + ".topic");
+    const std::string input_name_long =
+      declare_parameter<std::string>("input_channels." + selected_input_channel + ".name");
+    const std::string input_name_short =
+      declare_parameter<std::string>("input_channels." + selected_input_channel + ".name_short");
+    const double input_expected_frequency =
+      declare_parameter<double>("input_channels." + selected_input_channel + ".expected_frequency");
+    input_topic_names.push_back(input_topic_name);
+    input_names_long.push_back(input_name_long);
+    input_names_short.push_back(input_name_short);
+    input_expected_rates.push_back(input_expected_frequency);
   }
 
   input_channel_size_ = input_topic_names.size();
@@ -114,7 +121,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     input_channels_[i].input_topic = input_topic_names[i];
     input_channels_[i].long_name = input_names_long[i];
     input_channels_[i].short_name = input_names_short[i];
-    input_channels_[i].priority = static_cast<int>(input_priority[i]);
+    input_channels_[i].priority = 1;
+    input_channels_[i].expected_rate = input_expected_rates[i];
   }
 
   // Initialize input manager

From 5c621050fbe8e1a047101a9288b7a236a44a60c3 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 26 Apr 2024 12:42:27 +0900
Subject: [PATCH 38/67] feat: add input channels

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../config/input_channels.param.yaml          | 45 ++++++++++++++++---
 1 file changed, 38 insertions(+), 7 deletions(-)

diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml
index 09444dd30ec94..e13f2a2522544 100644
--- a/perception/multi_object_tracker/config/input_channels.param.yaml
+++ b/perception/multi_object_tracker/config/input_channels.param.yaml
@@ -6,31 +6,62 @@
         name: "detected_objects"
         name_short: "all"
         expected_frequency: 10.0
-      centerpoint:
+      # LIDAR - rule-based
+      lidar-clustering:
+        topic: "/perception/object_recognition/detection/clustering/objects"
+        name: "clustering"
+        name_short: "Lcl"
+        expected_frequency: 10.0
+      # LIDAR - DNN
+      lidar-centerpoint:
         topic: "/perception/object_recognition/detection/centerpoint/objects"
         name: "centerpoint"
         name_short: "Lcp"
         expected_frequency: 10.0
-      centerpoint_validated:
+      lidar-centerpoint_validated:
         topic: "/perception/object_recognition/detection/centerpoint/validation/objects"
         name: "centerpoint"
         name_short: "Lcp"
         expected_frequency: 10.0
-      camera_lidar_fusion:
-        topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects"
-        name: "camera_lidar_fusion"
-        name_short: "CLf"
+      lidar-apollo:
+        topic: "/perception/object_recognition/detection/apollo/objects"
+        name: "apollo"
+        name_short: "Lap"
         expected_frequency: 10.0
-      camera_lidar_fusion_filtered:
+      lidar-apollo_validated:
+        topic: "/perception/object_recognition/detection/apollo/validation/objects"
+        name: "apollo"
+        name_short: "Lap"
+        expected_frequency: 10.0
+      # LIDAR-CAMERA - DNN
+      pointpainitng:
+        topic: "/perception/object_recognition/detection/pointpainting/objects"
+        name: "pointpainting"
+        name_short: "Lpp"
+        expected_frequency: 10.0
+      pointpainting_validated:
+        topic: "/perception/object_recognition/detection/pointpainting/validation/objects"
+        name: "pointpainting"
+        name_short: "Lpp"
+        expected_frequency: 10.0
+      # CAMERA-LIDAR
+      camera_lidar_fusion:
         topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects"
         name: "camera_lidar_fusion"
         name_short: "CLf"
         expected_frequency: 10.0
+      # CAMERA-LIDAR+TRACKER
       detection_by_tracker:
         topic: "/perception/object_recognition/detection/detection_by_tracker/objects"
         name: "detection_by_tracker"
         name_short: "Ld"
         expected_frequency: 10.0
+      # RADAR
+      radar:
+        topic: "/sensing/radar/detected_objects"
+        name: "radar"
+        name_short: "R"
+        expected_frequency: 30.0
       radar_far:
         topic: "/perception/object_recognition/detection/radar/far_objects"
         name: "radar_far"

From 0d4b683640a06006627a8d662fc0da9eb41ce810 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 26 Apr 2024 14:28:52 +0900
Subject: [PATCH 39/67] fix: remove marker idx map

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../config/input_channels.param.yaml          | 10 ++--
 .../debugger/debug_object.hpp                 | 46 +------------------
 .../src/debugger/debug_object.cpp             | 34 +++++++-------
 .../src/tracker/model/tracker_base.cpp        |  4 +-
 4 files changed, 26 insertions(+), 68 deletions(-)

diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml
index e13f2a2522544..7e0a2a9dd8caa 100644
--- a/perception/multi_object_tracker/config/input_channels.param.yaml
+++ b/perception/multi_object_tracker/config/input_channels.param.yaml
@@ -7,28 +7,28 @@
         name_short: "all"
         expected_frequency: 10.0
       # LIDAR - rule-based
-      lidar-clustering:
+      lidar_clustering:
         topic: "/perception/object_recognition/detection/clustering/objects"
         name: "clustering"
         name_short: "Lcl"
         expected_frequency: 10.0
       # LIDAR - DNN
-      lidar-centerpoint:
+      lidar_centerpoint:
         topic: "/perception/object_recognition/detection/centerpoint/objects"
         name: "centerpoint"
         name_short: "Lcp"
         expected_frequency: 10.0
-      lidar-centerpoint_validated:
+      lidar_centerpoint_validated:
         topic: "/perception/object_recognition/detection/centerpoint/validation/objects"
         name: "centerpoint"
         name_short: "Lcp"
         expected_frequency: 10.0
-      lidar-apollo:
+      lidar_apollo:
         topic: "/perception/object_recognition/detection/apollo/objects"
         name: "apollo"
         name_short: "Lap"
         expected_frequency: 10.0
-      lidar-apollo_validated:
+      lidar_apollo_validated:
         topic: "/perception/object_recognition/detection/apollo/validation/objects"
         name: "apollo"
         name_short: "Lap"
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
index 44137c04a3580..c312edc843259 100644
--- 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
@@ -44,6 +44,7 @@ struct ObjectData
   // object uuid
   boost::uuids::uuid uuid;
   int uuid_int;
+  std::string uuid_str;
 
   // association link, pair of coordinates
   // tracker to detection
@@ -72,7 +73,6 @@ class TrackerObjectDebugger
   rclcpp::Time message_time_;
 
   std::vector<ObjectData> object_data_list_;
-  std::unordered_map<boost::uuids::uuid, int32_t, boost::hash<boost::uuids::uuid>> id_map_;
   std::list<int32_t> unused_marker_ids_;
   int32_t marker_id_ = 0;
   std::vector<std::vector<ObjectData>> object_data_groups_;
@@ -116,49 +116,7 @@ class TrackerObjectDebugger
     return uuid;
   }
 
-  void update_id_map(const std::vector<ObjectData> & object_data_list)
-  {
-    std::vector<boost::uuids::uuid> new_uuids;
-    std::vector<boost::uuids::uuid> tracked_uuids;
-    new_uuids.reserve(object_data_list.size());
-    tracked_uuids.reserve(object_data_list.size());
-
-    // check if the object is already tracked
-    for (const auto & object_data : object_data_list) {
-      ((id_map_.find(object_data.uuid) != id_map_.end()) ? tracked_uuids : new_uuids)
-        .push_back(object_data.uuid);
-    }
-
-    // remove untracked objects
-    auto itr = id_map_.begin();
-    while (itr != id_map_.end()) {
-      if (
-        std::find(tracked_uuids.begin(), tracked_uuids.end(), itr->first) == tracked_uuids.end()) {
-        unused_marker_ids_.push_back(itr->second);
-        itr = id_map_.erase(itr);
-      } else {
-        ++itr;
-      }
-    }
-
-    // add new objects
-    for (const auto & new_uuid : new_uuids) {
-      if (unused_marker_ids_.empty()) {
-        id_map_.emplace(new_uuid, marker_id_);
-        marker_id_++;
-      } else {
-        id_map_.emplace(new_uuid, unused_marker_ids_.front());
-        unused_marker_ids_.pop_front();
-      }
-    }
-  }
-
-  int32_t uuid_to_marker_id(const unique_identifier_msgs::msg::UUID & uuid_msg)
-  {
-    const auto uuid = to_boost_uuid(uuid_msg);
-    return id_map_.at(uuid);
-  }
-  int32_t uuid_to_marker_id(const boost::uuids::uuid & uuid) { return id_map_.at(uuid); }
+  int32_t uuid_to_int(const boost::uuids::uuid & uuid) { return boost::uuids::hash_value(uuid); }
 };
 
 #endif  // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_
diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp
index 3f8cb3badc9b1..11ecafba19e53 100644
--- a/perception/multi_object_tracker/src/debugger/debug_object.cpp
+++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp
@@ -68,6 +68,8 @@ void TrackerObjectDebugger::collect(
     autoware_auto_perception_msgs::msg::TrackedObject tracked_object;
     (*(tracker_itr))->getTrackedObject(message_time, tracked_object);
     object_data.uuid = to_boost_uuid(tracked_object.object_id);
+    object_data.uuid_int = uuid_to_int(object_data.uuid);
+    object_data.uuid_str = uuid_to_string(tracked_object.object_id);
 
     // tracker
     bool is_associated = false;
@@ -107,23 +109,18 @@ void TrackerObjectDebugger::process()
 {
   if (!is_initialized_) return;
 
-  // check all object data and update marker index
-  update_id_map(object_data_list_);
-
   // update uuid_int
   for (auto & object_data : object_data_list_) {
-    object_data.uuid_int = uuid_to_marker_id(object_data.uuid);
     current_ids_.insert(object_data.uuid_int);
   }
 
   // sort by uuid, collect the same uuid object_data as a group, and loop for the groups
-  // std::vector<std::vector<ObjectData>> object_data_groups;
   object_data_groups_.clear();
   {
-    // sort by uuid_int
+    // sort by uuid
     std::sort(
       object_data_list_.begin(), object_data_list_.end(),
-      [](const ObjectData & a, const ObjectData & b) { return a.uuid_int < b.uuid_int; });
+      [](const ObjectData & a, const ObjectData & b) { return a.uuid < b.uuid; });
 
     // collect the same uuid object_data as a group
     std::vector<ObjectData> object_data_group;
@@ -173,14 +170,14 @@ void TrackerObjectDebugger::draw(
 
   for (const auto & object_data_group : object_data_groups) {
     if (object_data_group.empty()) continue;
-    const int & uuid_int = object_data_group.front().uuid_int;
     const auto object_data_front = object_data_group.front();
     const auto object_data_back = object_data_group.back();
+
     // set a reference marker
     visualization_msgs::msg::Marker marker;
     marker.header.frame_id = frame_id_;
     marker.header.stamp = object_data_front.time;
-    marker.id = uuid_int;
+    marker.id = object_data_front.uuid_int;
     marker.pose.position.x = 0;
     marker.pose.position.y = 0;
     marker.pose.position.z = 0;
@@ -200,18 +197,20 @@ void TrackerObjectDebugger::draw(
     text_marker.scale.z = 0.5;
     text_marker.pose.position.x = object_data_front.tracker_point.x;
     text_marker.pose.position.y = object_data_front.tracker_point.y;
-    text_marker.pose.position.z = object_data_front.tracker_point.z + 2.0;
+    text_marker.pose.position.z = object_data_front.tracker_point.z + 2.5;
 
     // show the last existence probability
     // print existence probability with channel name
     // probability to text, two digits of percentage
     std::string existence_probability_text = "";
-    existence_probability_text += std::to_string(uuid_int);
     for (size_t i = 0; i < object_data_front.existence_vector.size(); ++i) {
       std::stringstream stream;
       stream << std::fixed << std::setprecision(0) << object_data_front.existence_vector[i] * 100;
-      existence_probability_text += ":" + channel_names_[i] + stream.str();
+      existence_probability_text += channel_names_[i] + stream.str() + ":";
     }
+    existence_probability_text =
+      existence_probability_text.substr(0, existence_probability_text.size() - 1);
+    existence_probability_text += "\n" + object_data_front.uuid_str.substr(0, 6);
 
     text_marker.text = existence_probability_text;
     marker_array.markers.push_back(text_marker);
@@ -219,7 +218,8 @@ void TrackerObjectDebugger::draw(
     // loop for each object_data in the group
     // boxed to tracker positions
     // and link lines to the detected positions
-    const double height_offset = 1.0;
+    const double marker_height_offset = 1.0;
+    const double assign_height_offset = 0.6;
 
     visualization_msgs::msg::Marker marker_track_boxes;
     marker_track_boxes = marker;
@@ -273,7 +273,7 @@ void TrackerObjectDebugger::draw(
       geometry_msgs::msg::Point box_point;
       box_point.x = object_data.tracker_point.x;
       box_point.y = object_data.tracker_point.y;
-      box_point.z = object_data.tracker_point.z + height_offset;
+      box_point.z = object_data.tracker_point.z + marker_height_offset;
       marker_track_boxes.points.push_back(box_point);
 
       // set association marker, if exists
@@ -285,7 +285,7 @@ void TrackerObjectDebugger::draw(
         marker_detect_boxes_per_channel.at(channel_id);
       box_point.x = object_data.detection_point.x;
       box_point.y = object_data.detection_point.y;
-      box_point.z = object_data.detection_point.z + height_offset + 1;
+      box_point.z = object_data.detection_point.z + marker_height_offset + assign_height_offset;
       marker_detect_boxes.points.push_back(box_point);
 
       // association line
@@ -294,11 +294,11 @@ void TrackerObjectDebugger::draw(
       geometry_msgs::msg::Point line_point;
       line_point.x = object_data.tracker_point.x;
       line_point.y = object_data.tracker_point.y;
-      line_point.z = object_data.tracker_point.z + height_offset;
+      line_point.z = object_data.tracker_point.z + marker_height_offset;
       marker_lines.points.push_back(line_point);
       line_point.x = object_data.detection_point.x;
       line_point.y = object_data.detection_point.y;
-      line_point.z = object_data.detection_point.z + height_offset + 1;
+      line_point.z = object_data.detection_point.z + marker_height_offset + assign_height_offset;
       marker_lines.points.push_back(line_point);
     }
 
diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
index 7c73ebae131d2..b62cf05449603 100644
--- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
+++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
@@ -62,8 +62,8 @@ bool Tracker::updateWithMeasurement(
     const double delta_time = (measurement_time - last_update_with_measurement_time_).seconds();
     const double decay_rate = 5.0 / 10.0;
 
-    const float gain = 0.8;
-    const float probability_detected = 0.8;
+    const float gain = 0.4;
+    const float probability_detected = 0.99;
     // existence_probabilities_[channel_index] = existence_probability_from_object;
     existence_probabilities_[channel_index] =
       gain * probability_detected + (1 - gain) * existence_probabilities_[channel_index];

From 9fbf5633ed72dc51e72683031517d66e6e58f7e9 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 26 Apr 2024 15:49:23 +0900
Subject: [PATCH 40/67] fix: to do not miss the latest message of the target
 stream

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker/config/input_channels.param.yaml | 6 +++---
 .../multi_object_tracker/src/processor/input_manager.cpp  | 8 ++++++++
 2 files changed, 11 insertions(+), 3 deletions(-)

diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml
index 7e0a2a9dd8caa..e48c2dfa6e281 100644
--- a/perception/multi_object_tracker/config/input_channels.param.yaml
+++ b/perception/multi_object_tracker/config/input_channels.param.yaml
@@ -34,12 +34,12 @@
         name_short: "Lap"
         expected_frequency: 10.0
       # LIDAR-CAMERA - DNN
-      pointpainitng:
+      lidar_pointpainitng:
         topic: "/perception/object_recognition/detection/pointpainting/objects"
         name: "pointpainting"
         name_short: "Lpp"
         expected_frequency: 10.0
-      pointpainting_validated:
+      lidar_pointpainting_validated:
         topic: "/perception/object_recognition/detection/pointpainting/validation/objects"
         name: "pointpainting"
         name_short: "Lpp"
@@ -54,7 +54,7 @@
       detection_by_tracker:
         topic: "/perception/object_recognition/detection/detection_by_tracker/objects"
         name: "detection_by_tracker"
-        name_short: "Ld"
+        name_short: "dbT"
         expected_frequency: 10.0
       # RADAR
       radar:
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 39a6f5d253871..c4d1f1dfcd94f 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -184,6 +184,14 @@ void InputManager::getObjectTimeInterval(
   object_latest_time = now - rclcpp::Duration::from_seconds(target_latency_);
   object_oldest_time = now - rclcpp::Duration::from_seconds(target_latency_ + target_latency_band_);
 
+  // get the latest timestamp of the target stream
+  rclcpp::Time latest_message_time, latest_measurement_time;
+  if (input_streams_.at(target_stream_idx_)
+        ->getTimestamps(latest_measurement_time, latest_message_time)) {
+    object_latest_time =
+      object_latest_time > latest_message_time ? object_latest_time : latest_message_time;
+  }
+
   // if the object_oldest_time is older than the latest object time, set it to the latest object
   // time
   object_oldest_time =

From 736411dbacc7254796d253ae8091e459c460a7d8 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 26 Apr 2024 16:58:28 +0900
Subject: [PATCH 41/67] fix: remove priority, separate timing optimization

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../processor/input_manager.hpp               |  9 ++--
 .../src/multi_object_tracker_core.cpp         |  1 -
 .../src/processor/input_manager.cpp           | 50 +++++++++++--------
 3 files changed, 33 insertions(+), 27 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 701a01a6dc0c1..2c5b11bd2bcbf 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -38,7 +38,6 @@ struct InputChannel
   std::string short_name = "DET";             // abbreviation of the name
   double expected_rate = 10.0;                // [Hz]
   double expected_latency = 0.2;              // [s]
-  int priority = 0;                           // priority of the input channel, higher is better
 };
 
 class InputStream
@@ -56,8 +55,8 @@ class InputStream
 
   void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
 
+  bool isTimeInitialized() const { return is_time_initialized_; }
   uint getIndex() const { return index_; }
-  int getPriority() const { return priority_; }
   void getObjectsOlderThan(
     const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
     ObjectsList & objects_list);
@@ -88,8 +87,6 @@ class InputStream
   std::string long_name_;
   std::string short_name_;
 
-  int priority_{0};
-
   size_t que_size_{30};
   std::deque<DetectedObjects> objects_que_;
 
@@ -117,7 +114,9 @@ class InputManager
   void onTrigger(const uint & index) const;
 
   void getObjectTimeInterval(
-    const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time);
+    const rclcpp::Time & now, rclcpp::Time & object_latest_time,
+    rclcpp::Time & object_oldest_time) const;
+  void optimizeTimings();
   bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list);
 
 private:
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 7af2922dfd334..8ba66d92d1e21 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -121,7 +121,6 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     input_channels_[i].input_topic = input_topic_names[i];
     input_channels_[i].long_name = input_names_long[i];
     input_channels_[i].short_name = input_names_short[i];
-    input_channels_[i].priority = 1;
     input_channels_[i].expected_rate = input_expected_rates[i];
   }
 
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index c4d1f1dfcd94f..6102c6f1cc854 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -18,7 +18,9 @@
 
 namespace multi_object_tracker
 {
-
+///////////////////////////
+/////// InputStream ///////
+///////////////////////////
 InputStream::InputStream(rclcpp::Node & node, uint & index) : node_(node), index_(index)
 {
 }
@@ -30,8 +32,6 @@ void InputStream::init(const InputChannel & input_channel)
   long_name_ = input_channel.long_name;
   short_name_ = input_channel.short_name;
 
-  priority_ = input_channel.priority;
-
   // Initialize queue
   objects_que_.clear();
 
@@ -133,6 +133,9 @@ void InputStream::getObjectsOlderThan(
   }
 }
 
+////////////////////////////
+/////// InputManager ///////
+////////////////////////////
 InputManager::InputManager(rclcpp::Node & node) : node_(node)
 {
   latest_object_time_ = node_.now();
@@ -179,12 +182,13 @@ void InputManager::onTrigger(const uint & index) const
 }
 
 void InputManager::getObjectTimeInterval(
-  const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time)
+  const rclcpp::Time & now, rclcpp::Time & object_latest_time,
+  rclcpp::Time & object_oldest_time) const
 {
   object_latest_time = now - rclcpp::Duration::from_seconds(target_latency_);
   object_oldest_time = now - rclcpp::Duration::from_seconds(target_latency_ + target_latency_band_);
 
-  // get the latest timestamp of the target stream
+  // try to include the latest message of the target stream
   rclcpp::Time latest_message_time, latest_measurement_time;
   if (input_streams_.at(target_stream_idx_)
         ->getTimestamps(latest_measurement_time, latest_message_time)) {
@@ -196,8 +200,10 @@ void InputManager::getObjectTimeInterval(
   // time
   object_oldest_time =
     object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_;
+}
 
-  // OPTIMIZATION: Get the stream with the maximum latency
+void InputManager::optimizeTimings()
+{
   double max_latency_mean = 0.0;
   uint stream_selected_idx = 0;
   double selected_idx_latency_std = 0.0;
@@ -205,25 +211,24 @@ void InputManager::getObjectTimeInterval(
 
   {
     // ANALYSIS: Get the streams statistics
-    std::string long_name, short_name;
     double latency_mean, latency_var, interval_mean, interval_var;
-    rclcpp::Time latest_measurement_time, latest_message_time;
     for (const auto & input_stream : input_streams_) {
-      input_stream->getNames(long_name, short_name);
+      if (!input_stream->isTimeInitialized()) continue;
       input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var);
-      if (!input_stream->getTimestamps(latest_measurement_time, latest_message_time)) {
-        continue;
-      }
-      double latency_message = (now - latest_message_time).seconds();
-      double latency_measurement = (now - latest_measurement_time).seconds();
-      if (latency_mean > max_latency_mean && input_stream->getPriority() > 0) {
+      if (latency_mean > max_latency_mean) {
         max_latency_mean = latency_mean;
         selected_idx_latency_std = std::sqrt(latency_var);
         stream_selected_idx = input_stream->getIndex();
         selected_idx_interval = interval_mean;
       }
 
-      // print info
+      /* DEBUG */
+      std::string long_name, short_name;
+      rclcpp::Time latest_measurement_time, latest_message_time;
+      input_stream->getNames(long_name, short_name);
+      input_stream->getTimestamps(latest_measurement_time, latest_message_time);
+      double latency_message = (node_.now() - latest_message_time).seconds();
+      double latency_measurement = (node_.now() - latest_measurement_time).seconds();
       RCLCPP_INFO(
         node_.get_logger(),
         "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: "
@@ -238,6 +243,8 @@ void InputManager::getObjectTimeInterval(
   target_stream_idx_ = stream_selected_idx;
   target_latency_ = max_latency_mean - selected_idx_latency_std;
   target_latency_band_ = 2 * selected_idx_latency_std + selected_idx_interval;
+
+  /* DEBUG */
   RCLCPP_INFO(
     node_.get_logger(), "InputManager::getObjects Target stream: %s, target latency: %f, band: %f",
     input_streams_.at(target_stream_idx_)->getLongName().c_str(), target_latency_,
@@ -258,8 +265,13 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
   rclcpp::Time object_latest_time, object_oldest_time;
   getObjectTimeInterval(now, object_latest_time, object_oldest_time);
 
+  // Optimize the target stream, latency, and its band
+  // The result will be used for the next time, so the optimization is after getting the time
+  // interval
+  optimizeTimings();
+
   // Get objects from all input streams
-  // adds-up to the objects vector for efficient processing
+  // adds up to the objects vector for efficient processing
   for (const auto & input_stream : input_streams_) {
     input_stream->getObjectsOlderThan(object_latest_time, object_oldest_time, objects_list);
   }
@@ -272,10 +284,6 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
              0;
     });
 
-  RCLCPP_INFO(
-    node_.get_logger(), "InputManager::getObjects Got %zu objects from input streams",
-    objects_list.size());
-
   // Update the latest object time
   if (!objects_list.empty()) {
     latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp);

From 5a0757e59429936115cfc2d9097d496a5bd04ef7 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 26 Apr 2024 18:25:24 +0900
Subject: [PATCH 42/67] fix: time interval bug fix

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker/processor/input_manager.hpp      | 1 +
 .../multi_object_tracker/src/processor/input_manager.cpp  | 8 ++++----
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 2c5b11bd2bcbf..fa5a750ed35db 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -78,6 +78,7 @@ class InputStream
   }
   bool getTimestamps(
     rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const;
+  rclcpp::Time getLatestMeasurementTime() const { return latest_measurement_time_; }
 
 private:
   rclcpp::Node & node_;
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 6102c6f1cc854..47ae1b2dc364b 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -189,11 +189,11 @@ void InputManager::getObjectTimeInterval(
   object_oldest_time = now - rclcpp::Duration::from_seconds(target_latency_ + target_latency_band_);
 
   // try to include the latest message of the target stream
-  rclcpp::Time latest_message_time, latest_measurement_time;
-  if (input_streams_.at(target_stream_idx_)
-        ->getTimestamps(latest_measurement_time, latest_message_time)) {
+  if (input_streams_.at(target_stream_idx_)->isTimeInitialized()) {
+    rclcpp::Time latest_measurement_time =
+      input_streams_.at(target_stream_idx_)->getLatestMeasurementTime();
     object_latest_time =
-      object_latest_time > latest_message_time ? object_latest_time : latest_message_time;
+      object_latest_time > latest_measurement_time ? object_latest_time : latest_measurement_time;
   }
 
   // if the object_oldest_time is older than the latest object time, set it to the latest object

From 5437a9e9d3a2d87cfc977509432f6e8d4b51143d Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 26 Apr 2024 18:28:47 +0900
Subject: [PATCH 43/67] chore: refactoring timing state update

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../processor/input_manager.hpp               |  1 +
 .../src/processor/input_manager.cpp           | 24 +++++++++++--------
 2 files changed, 15 insertions(+), 10 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index fa5a750ed35db..cd9181e12d80a 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -54,6 +54,7 @@ class InputStream
   }
 
   void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
+  void updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time);
 
   bool isTimeInitialized() const { return is_time_initialized_; }
   uint getIndex() const { return index_; }
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 47ae1b2dc364b..25e8f46f19033 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -66,9 +66,21 @@ void InputStream::onMessage(
     objects_que_.pop_front();
   }
 
+  // update the timing statistics
+  rclcpp::Time now = node_.now();
+  rclcpp::Time objects_time(objects.header.stamp);
+  updateTimingStatus(now, objects_time);
+
+  // trigger the function if it is set
+  if (func_trigger_) {
+    func_trigger_(index_);
+  }
+}
+
+void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time)
+{
   // Filter parameters
   constexpr double gain = 0.05;
-  const auto now = node_.now();
 
   // Calculate interval, Update interval statistics
   if (is_time_initialized_) {
@@ -89,23 +101,15 @@ void InputStream::onMessage(
 
   // Update time
   latest_message_time_ = now;
-  rclcpp::Time objects_time(objects.header.stamp);
   latest_measurement_time_ =
     latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_;
   if (!is_time_initialized_) is_time_initialized_ = true;
 
-  // Calculate latency
-  const double latency = (latest_message_time_ - objects_time).seconds();
-
   // Update latency statistics
+  const double latency = (latest_message_time_ - objects_time).seconds();
   latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency;
   const double latency_delta = latency - latency_mean_;
   latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta;
-
-  // trigger the function if it is set
-  if (func_trigger_) {
-    func_trigger_(index_);
-  }
 }
 
 void InputStream::getObjectsOlderThan(

From 6d317e5907c65bc473216b1bc01d5a73a617a738 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Tue, 7 May 2024 14:36:18 +0900
Subject: [PATCH 44/67] fix: set parameters optionally

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../config/input_channels.param.yaml          | 60 +++++++++++--------
 .../src/multi_object_tracker_core.cpp         | 27 ++++++---
 2 files changed, 54 insertions(+), 33 deletions(-)

diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml
index e48c2dfa6e281..23c567d525508 100644
--- a/perception/multi_object_tracker/config/input_channels.param.yaml
+++ b/perception/multi_object_tracker/config/input_channels.param.yaml
@@ -3,67 +3,79 @@
     input_channels:
       detected_objects:
         topic: "/perception/object_recognition/detection/objects"
-        name: "detected_objects"
-        name_short: "all"
         expected_frequency: 10.0
+        optional:
+          name: "detected_objects"
+          short_name: "all"
       # LIDAR - rule-based
       lidar_clustering:
         topic: "/perception/object_recognition/detection/clustering/objects"
-        name: "clustering"
-        name_short: "Lcl"
         expected_frequency: 10.0
+        optional:
+          name: "clustering"
+          short_name: "Lcl"
       # LIDAR - DNN
       lidar_centerpoint:
         topic: "/perception/object_recognition/detection/centerpoint/objects"
-        name: "centerpoint"
-        name_short: "Lcp"
         expected_frequency: 10.0
+        optional:
+          name: "centerpoint"
+          short_name: "Lcp"
       lidar_centerpoint_validated:
         topic: "/perception/object_recognition/detection/centerpoint/validation/objects"
-        name: "centerpoint"
-        name_short: "Lcp"
         expected_frequency: 10.0
+        optional:
+          name: "centerpoint"
+          short_name: "Lcp"
       lidar_apollo:
         topic: "/perception/object_recognition/detection/apollo/objects"
-        name: "apollo"
-        name_short: "Lap"
         expected_frequency: 10.0
+        optional:
+          name: "apollo"
+          short_name: "Lap"
       lidar_apollo_validated:
         topic: "/perception/object_recognition/detection/apollo/validation/objects"
-        name: "apollo"
-        name_short: "Lap"
         expected_frequency: 10.0
+        optional:
+          name: "apollo"
+          short_name: "Lap"
       # LIDAR-CAMERA - DNN
       lidar_pointpainitng:
         topic: "/perception/object_recognition/detection/pointpainting/objects"
-        name: "pointpainting"
-        name_short: "Lpp"
         expected_frequency: 10.0
+        optional:
+          name: "pointpainting"
+          short_name: "Lpp"
       lidar_pointpainting_validated:
         topic: "/perception/object_recognition/detection/pointpainting/validation/objects"
-        name: "pointpainting"
-        name_short: "Lpp"
         expected_frequency: 10.0
+        optional:
+          name: "pointpainting"
+          short_name: "Lpp"
       # CAMERA-LIDAR
       camera_lidar_fusion:
         topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects"
-        name: "camera_lidar_fusion"
-        name_short: "CLf"
         expected_frequency: 10.0
+        optional:
+          name: "camera_lidar_fusion"
+          short_name: "CLf"
       # CAMERA-LIDAR+TRACKER
       detection_by_tracker:
         topic: "/perception/object_recognition/detection/detection_by_tracker/objects"
-        name: "detection_by_tracker"
-        name_short: "dbT"
         expected_frequency: 10.0
+        optional:
+          name: "detection_by_tracker"
+          short_name: "dbT"
       # RADAR
       radar:
         topic: "/sensing/radar/detected_objects"
-        name: "radar"
-        name_short: "R"
         expected_frequency: 30.0
+        optional:
+          name: "radar"
+          short_name: "R"
       radar_far:
         topic: "/perception/object_recognition/detection/radar/far_objects"
-        name: "radar_far"
-        name_short: "Rf"
         expected_frequency: 30.0
+        optional:
+          name: "radar_far"
+          short_name: "Rf"
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 8ba66d92d1e21..1de771b354745 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -100,18 +100,27 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   }
 
   for (const auto & selected_input_channel : selected_input_channels) {
+    // required parameters, no default value
     const std::string input_topic_name =
       declare_parameter<std::string>("input_channels." + selected_input_channel + ".topic");
-    const std::string input_name_long =
-      declare_parameter<std::string>("input_channels." + selected_input_channel + ".name");
-    const std::string input_name_short =
-      declare_parameter<std::string>("input_channels." + selected_input_channel + ".name_short");
-    const double input_expected_frequency =
-      declare_parameter<double>("input_channels." + selected_input_channel + ".expected_frequency");
     input_topic_names.push_back(input_topic_name);
-    input_names_long.push_back(input_name_long);
-    input_names_short.push_back(input_name_short);
-    input_expected_rates.push_back(input_expected_frequency);
+
+    // required parameter, but can set a default value
+    const double default_expected_rate = 10.0;
+    const double expected_rate = declare_parameter<double>(
+      "input_channels." + selected_input_channel + ".expected_rate", default_expected_rate);
+    input_expected_rates.push_back(expected_rate);
+
+    // optional parameters
+    const std::string default_name = selected_input_channel;
+    const std::string name_long = declare_parameter<std::string>(
+      "input_channels." + selected_input_channel + ".optional.name", default_name);
+    input_names_long.push_back(name_long);
+
+    const std::string default_name_short = selected_input_channel.substr(0, 3);
+    const std::string name_short = declare_parameter<std::string>(
+      "input_channels." + selected_input_channel + ".optional.short_name", default_name_short);
+    input_names_short.push_back(name_short);
   }
 
   input_channel_size_ = input_topic_names.size();

From adc84154f4cf0c8f08b3d26a5ddbeb8348437015 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Tue, 7 May 2024 19:13:34 +0900
Subject: [PATCH 45/67] feat: revise object time range logic

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../config/input_channels.param.yaml          | 24 +++----
 .../processor/input_manager.hpp               | 17 +++--
 .../src/multi_object_tracker_core.cpp         | 12 ++--
 .../src/processor/input_manager.cpp           | 70 +++++++++++--------
 4 files changed, 73 insertions(+), 50 deletions(-)

diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml
index 23c567d525508..63de901ef0c11 100644
--- a/perception/multi_object_tracker/config/input_channels.param.yaml
+++ b/perception/multi_object_tracker/config/input_channels.param.yaml
@@ -3,79 +3,79 @@
     input_channels:
       detected_objects:
         topic: "/perception/object_recognition/detection/objects"
-        expected_frequency: 10.0
+        expected_interval: 0.1
         optional:
           name: "detected_objects"
           short_name: "all"
       # LIDAR - rule-based
       lidar_clustering:
         topic: "/perception/object_recognition/detection/clustering/objects"
-        expected_frequency: 10.0
+        expected_interval: 0.1
         optional:
           name: "clustering"
           short_name: "Lcl"
       # LIDAR - DNN
       lidar_centerpoint:
         topic: "/perception/object_recognition/detection/centerpoint/objects"
-        expected_frequency: 10.0
+        expected_interval: 0.1
         optional:
           name: "centerpoint"
           short_name: "Lcp"
       lidar_centerpoint_validated:
         topic: "/perception/object_recognition/detection/centerpoint/validation/objects"
-        expected_frequency: 10.0
+        expected_interval: 0.1
         optional:
           name: "centerpoint"
           short_name: "Lcp"
       lidar_apollo:
         topic: "/perception/object_recognition/detection/apollo/objects"
-        expected_frequency: 10.0
+        expected_interval: 0.1
         optional:
           name: "apollo"
           short_name: "Lap"
       lidar_apollo_validated:
         topic: "/perception/object_recognition/detection/apollo/validation/objects"
-        expected_frequency: 10.0
+        expected_interval: 0.1
         optional:
           name: "apollo"
           short_name: "Lap"
       # LIDAR-CAMERA - DNN
       lidar_pointpainitng:
         topic: "/perception/object_recognition/detection/pointpainting/objects"
-        expected_frequency: 10.0
+        expected_interval: 0.1
         optional:
           name: "pointpainting"
           short_name: "Lpp"
       lidar_pointpainting_validated:
         topic: "/perception/object_recognition/detection/pointpainting/validation/objects"
-        expected_frequency: 10.0
+        expected_interval: 0.1
         optional:
           name: "pointpainting"
           short_name: "Lpp"
       # CAMERA-LIDAR
       camera_lidar_fusion:
         topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects"
-        expected_frequency: 10.0
+        expected_interval: 0.1
         optional:
           name: "camera_lidar_fusion"
           short_name: "CLf"
       # CAMERA-LIDAR+TRACKER
       detection_by_tracker:
         topic: "/perception/object_recognition/detection/detection_by_tracker/objects"
-        expected_frequency: 10.0
+        expected_interval: 0.1
         optional:
           name: "detection_by_tracker"
           short_name: "dbT"
       # RADAR
       radar:
         topic: "/sensing/radar/detected_objects"
-        expected_frequency: 30.0
+        expected_interval: 0.0333
         optional:
           name: "radar"
           short_name: "R"
       radar_far:
         topic: "/perception/object_recognition/detection/radar/far_objects"
-        expected_frequency: 30.0
+        expected_interval: 0.0333
         optional:
           name: "radar_far"
           short_name: "Rf"
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index cd9181e12d80a..ce2b2075c99a2 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -36,7 +36,7 @@ struct InputChannel
   std::string input_topic;  // topic name of the detection, e.g. "/detection/lidar"
   std::string long_name = "Detected Object";  // full name of the detection
   std::string short_name = "DET";             // abbreviation of the name
-  double expected_rate = 10.0;                // [Hz]
+  double expected_interval = 0.1;             // [s]
   double expected_latency = 0.2;              // [s]
 };
 
@@ -77,6 +77,11 @@ class InputStream
     interval_mean = interval_mean_;
     interval_var = interval_var_;
   }
+  void getLatencyStatistics(double & latency_mean, double & latency_var) const
+  {
+    latency_mean = latency_mean_;
+    latency_var = latency_var_;
+  }
   bool getTimestamps(
     rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const;
   rclcpp::Time getLatestMeasurementTime() const { return latest_measurement_time_; }
@@ -95,7 +100,7 @@ class InputStream
   std::function<void(const uint &)> func_trigger_;
 
   bool is_time_initialized_{false};
-  double expected_rate_;
+  double expected_interval_;
   double latency_mean_;
   double latency_var_;
   double interval_mean_;
@@ -133,8 +138,12 @@ class InputManager
 
   std::function<void()> func_trigger_;
   uint target_stream_idx_{0};
-  double target_latency_{0.2};
-  double target_latency_band_{0.14};
+  double target_stream_latency_{0.2};        // [s]
+  double target_stream_latency_std_{0.04};   // [s]
+  double target_stream_interval_{0.1};       // [s]
+  double target_stream_interval_std_{0.02};  // [s]
+  double target_latency_{0.2};               // [s]
+  double target_latency_band_{1.0};          // [s]
 };
 
 }  // namespace multi_object_tracker
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 1de771b354745..8d8a10266fec7 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -92,7 +92,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   std::vector<std::string> input_topic_names;
   std::vector<std::string> input_names_long;
   std::vector<std::string> input_names_short;
-  std::vector<double> input_expected_rates;
+  std::vector<double> input_expected_intervals;
 
   if (selected_input_channels.empty()) {
     RCLCPP_ERROR(this->get_logger(), "No input topics are specified.");
@@ -106,10 +106,10 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     input_topic_names.push_back(input_topic_name);
 
     // required parameter, but can set a default value
-    const double default_expected_rate = 10.0;
-    const double expected_rate = declare_parameter<double>(
-      "input_channels." + selected_input_channel + ".expected_rate", default_expected_rate);
-    input_expected_rates.push_back(expected_rate);
+    const double default_expected_interval = 0.1;  // [s]
+    const double expected_interval = declare_parameter<double>(
+      "input_channels." + selected_input_channel + ".expected_interval", default_expected_interval);
+    input_expected_intervals.push_back(expected_interval);
 
     // optional parameters
     const std::string default_name = selected_input_channel;
@@ -130,7 +130,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     input_channels_[i].input_topic = input_topic_names[i];
     input_channels_[i].long_name = input_names_long[i];
     input_channels_[i].short_name = input_names_short[i];
-    input_channels_[i].expected_rate = input_expected_rates[i];
+    input_channels_[i].expected_interval = input_expected_intervals[i];
   }
 
   // Initialize input manager
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 25e8f46f19033..bf066e23828f8 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -31,15 +31,15 @@ void InputStream::init(const InputChannel & input_channel)
   input_topic_ = input_channel.input_topic;
   long_name_ = input_channel.long_name;
   short_name_ = input_channel.short_name;
+  expected_interval_ = input_channel.expected_interval;  // [s]
 
   // Initialize queue
   objects_que_.clear();
 
   // Initialize latency statistics
-  expected_rate_ = input_channel.expected_rate;    // [Hz]
   latency_mean_ = input_channel.expected_latency;  // [s]
   latency_var_ = 0.0;
-  interval_mean_ = 1 / expected_rate_;
+  interval_mean_ = expected_interval_;  // [s] (initial value)
   interval_var_ = 0.0;
 
   latest_measurement_time_ = node_.now();
@@ -84,13 +84,11 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim
 
   // Calculate interval, Update interval statistics
   if (is_time_initialized_) {
-    bool is_interval_regular = true;
     const double interval = (now - latest_message_time_).seconds();
-    // check if it is outlier
-    if (interval > 1.5 * interval_mean_ || interval < 0.5 * interval_mean_) {
-      // no update for the time statistics
-      is_interval_regular = false;
-    }
+    // Check if the interval is regular
+    // The interval is considered regular if it is within 0.5 and 1.5 times the expected interval
+    bool is_interval_regular =
+      interval > 0.5 * expected_interval_ && interval < 1.5 * expected_interval_;
 
     if (is_interval_regular) {
       interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval;
@@ -189,17 +187,33 @@ void InputManager::getObjectTimeInterval(
   const rclcpp::Time & now, rclcpp::Time & object_latest_time,
   rclcpp::Time & object_oldest_time) const
 {
-  object_latest_time = now - rclcpp::Duration::from_seconds(target_latency_);
-  object_oldest_time = now - rclcpp::Duration::from_seconds(target_latency_ + target_latency_band_);
-
-  // try to include the latest message of the target stream
+  object_latest_time =
+    now - rclcpp::Duration::from_seconds(
+            target_stream_latency_ -
+            0.1 * target_stream_latency_std_);  // object_latest_time with 0.1 sigma safety margin
+  // check the target stream can be included in the object time interval
   if (input_streams_.at(target_stream_idx_)->isTimeInitialized()) {
     rclcpp::Time latest_measurement_time =
       input_streams_.at(target_stream_idx_)->getLatestMeasurementTime();
+    // if the object_latest_time is newer than the next expected message time, set it older
+    // than the next expected message time
+    rclcpp::Time next_expected_message_time =
+      latest_measurement_time +
+      rclcpp::Duration::from_seconds(
+        target_stream_interval_ -
+        1.0 *
+          target_stream_interval_std_);  // next expected message time with 1 sigma safety margin
+    object_latest_time = object_latest_time > next_expected_message_time
+                           ? next_expected_message_time
+                           : object_latest_time;
+
+    // if the object_latest_time is older than the latest measurement time, set it as the latest
+    // object time
     object_latest_time =
-      object_latest_time > latest_measurement_time ? object_latest_time : latest_measurement_time;
+      object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time;
   }
 
+  object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0);
   // if the object_oldest_time is older than the latest object time, set it to the latest object
   // time
   object_oldest_time =
@@ -209,21 +223,24 @@ void InputManager::getObjectTimeInterval(
 void InputManager::optimizeTimings()
 {
   double max_latency_mean = 0.0;
-  uint stream_selected_idx = 0;
-  double selected_idx_latency_std = 0.0;
-  double selected_idx_interval = 0.0;
+  uint selected_stream_idx = 0;
+  double selected_stream_latency_std = 0.1;
+  double selected_stream_interval = 0.1;
+  double selected_stream_interval_std = 0.01;
 
   {
     // ANALYSIS: Get the streams statistics
+    // select the stream that has the maximum latency
     double latency_mean, latency_var, interval_mean, interval_var;
     for (const auto & input_stream : input_streams_) {
       if (!input_stream->isTimeInitialized()) continue;
       input_stream->getTimeStatistics(latency_mean, latency_var, interval_mean, interval_var);
       if (latency_mean > max_latency_mean) {
         max_latency_mean = latency_mean;
-        selected_idx_latency_std = std::sqrt(latency_var);
-        stream_selected_idx = input_stream->getIndex();
-        selected_idx_interval = interval_mean;
+        selected_stream_idx = input_stream->getIndex();
+        selected_stream_latency_std = std::sqrt(latency_var);
+        selected_stream_interval = interval_mean;
+        selected_stream_interval_std = std::sqrt(interval_var);
       }
 
       /* DEBUG */
@@ -244,15 +261,12 @@ void InputManager::optimizeTimings()
 
   // Set the target stream index, which has the maximum latency
   // trigger will be called next time
-  target_stream_idx_ = stream_selected_idx;
-  target_latency_ = max_latency_mean - selected_idx_latency_std;
-  target_latency_band_ = 2 * selected_idx_latency_std + selected_idx_interval;
-
-  /* DEBUG */
-  RCLCPP_INFO(
-    node_.get_logger(), "InputManager::getObjects Target stream: %s, target latency: %f, band: %f",
-    input_streams_.at(target_stream_idx_)->getLongName().c_str(), target_latency_,
-    target_latency_band_);
+  // if no stream is initialized, the target stream index will be 0 and wait for the initialization
+  target_stream_idx_ = selected_stream_idx;
+  target_stream_latency_ = max_latency_mean;
+  target_stream_latency_std_ = selected_stream_latency_std;
+  target_stream_interval_ = selected_stream_interval;
+  target_stream_interval_std_ = selected_stream_interval_std;
 }
 
 bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_list)

From ca5c4ac7c205bfedfb5c3778c0674502fa08d0b5 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Wed, 8 May 2024 13:02:43 +0900
Subject: [PATCH 46/67] fix: launcher to set input channels

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../launch/object_recognition/tracking/tracking.launch.xml      | 2 ++
 launch/tier4_perception_launch/launch/perception.launch.xml     | 1 +
 2 files changed, 3 insertions(+)

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
index fed268084792a..cf11c65ac805c 100644
--- 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
index 71fb38f883144..9b9cd6b7f077f 100644
--- 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"/>

From b2ca6a1607661ef11854e8cf7c987c7d88988e7a Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 10 May 2024 11:08:11 +0900
Subject: [PATCH 47/67] fix: exempt spell check 'pointpainting'

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 perception/multi_object_tracker/config/input_channels.param.yaml | 1 +
 1 file changed, 1 insertion(+)

diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml
index 63de901ef0c11..9d9b3ae77367f 100644
--- a/perception/multi_object_tracker/config/input_channels.param.yaml
+++ b/perception/multi_object_tracker/config/input_channels.param.yaml
@@ -40,6 +40,7 @@
           name: "apollo"
           short_name: "Lap"
       # LIDAR-CAMERA - DNN
+      # cspell:ignore lidar_pointpainitng pointpainting
       lidar_pointpainitng:
         topic: "/perception/object_recognition/detection/pointpainting/objects"
         expected_interval: 0.1

From df0cd9fd9921d41e4b6fe9433f4624ecafbb7fc1 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 13 May 2024 13:54:43 +0900
Subject: [PATCH 48/67] feat: remove expected interval

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../config/input_channels.param.yaml          | 23 +++----
 .../processor/input_manager.hpp               |  8 +--
 .../src/multi_object_tracker_core.cpp         | 11 ++-
 .../src/processor/input_manager.cpp           | 69 ++++++++++---------
 4 files changed, 56 insertions(+), 55 deletions(-)

diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml
index 9d9b3ae77367f..20d717ee36e1f 100644
--- a/perception/multi_object_tracker/config/input_channels.param.yaml
+++ b/perception/multi_object_tracker/config/input_channels.param.yaml
@@ -3,39 +3,39 @@
     input_channels:
       detected_objects:
         topic: "/perception/object_recognition/detection/objects"
-        expected_interval: 0.1
+        can_spawn_new_tracker: true
         optional:
           name: "detected_objects"
           short_name: "all"
       # LIDAR - rule-based
       lidar_clustering:
         topic: "/perception/object_recognition/detection/clustering/objects"
-        expected_interval: 0.1
+        can_spawn_new_tracker: true
         optional:
           name: "clustering"
           short_name: "Lcl"
       # LIDAR - DNN
       lidar_centerpoint:
         topic: "/perception/object_recognition/detection/centerpoint/objects"
-        expected_interval: 0.1
+        can_spawn_new_tracker: true
         optional:
           name: "centerpoint"
           short_name: "Lcp"
       lidar_centerpoint_validated:
         topic: "/perception/object_recognition/detection/centerpoint/validation/objects"
-        expected_interval: 0.1
+        can_spawn_new_tracker: true
         optional:
           name: "centerpoint"
           short_name: "Lcp"
       lidar_apollo:
         topic: "/perception/object_recognition/detection/apollo/objects"
-        expected_interval: 0.1
+        can_spawn_new_tracker: true
         optional:
           name: "apollo"
           short_name: "Lap"
       lidar_apollo_validated:
         topic: "/perception/object_recognition/detection/apollo/validation/objects"
-        expected_interval: 0.1
+        can_spawn_new_tracker: true
         optional:
           name: "apollo"
           short_name: "Lap"
@@ -43,40 +43,39 @@
       # cspell:ignore lidar_pointpainitng pointpainting
       lidar_pointpainitng:
         topic: "/perception/object_recognition/detection/pointpainting/objects"
-        expected_interval: 0.1
+        can_spawn_new_tracker: true
         optional:
           name: "pointpainting"
           short_name: "Lpp"
       lidar_pointpainting_validated:
         topic: "/perception/object_recognition/detection/pointpainting/validation/objects"
-        expected_interval: 0.1
         optional:
           name: "pointpainting"
           short_name: "Lpp"
       # CAMERA-LIDAR
       camera_lidar_fusion:
         topic: "/perception/object_recognition/detection/clustering/camera_lidar_fusion/objects"
-        expected_interval: 0.1
+        can_spawn_new_tracker: false
         optional:
           name: "camera_lidar_fusion"
           short_name: "CLf"
       # CAMERA-LIDAR+TRACKER
       detection_by_tracker:
         topic: "/perception/object_recognition/detection/detection_by_tracker/objects"
-        expected_interval: 0.1
+        can_spawn_new_tracker: false
         optional:
           name: "detection_by_tracker"
           short_name: "dbT"
       # RADAR
       radar:
         topic: "/sensing/radar/detected_objects"
-        expected_interval: 0.0333
+        can_spawn_new_tracker: true
         optional:
           name: "radar"
           short_name: "R"
       radar_far:
         topic: "/perception/object_recognition/detection/radar/far_objects"
-        expected_interval: 0.0333
+        can_spawn_new_tracker: true
         optional:
           name: "radar_far"
           short_name: "Rf"
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index ce2b2075c99a2..03465cb56adae 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -36,8 +36,7 @@ struct InputChannel
   std::string input_topic;  // topic name of the detection, e.g. "/detection/lidar"
   std::string long_name = "Detected Object";  // full name of the detection
   std::string short_name = "DET";             // abbreviation of the name
-  double expected_interval = 0.1;             // [s]
-  double expected_latency = 0.2;              // [s]
+  bool is_spawn_enabled = true;               // enable spawn of the object
 };
 
 class InputStream
@@ -56,7 +55,7 @@ class InputStream
   void onMessage(const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg);
   void updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time);
 
-  bool isTimeInitialized() const { return is_time_initialized_; }
+  bool isTimeInitialized() const { return initial_count_ > 0; }
   uint getIndex() const { return index_; }
   void getObjectsOlderThan(
     const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
@@ -99,7 +98,8 @@ class InputStream
 
   std::function<void(const uint &)> func_trigger_;
 
-  bool is_time_initialized_{false};
+  // bool is_time_initialized_{false};
+  int initial_count_{0};
   double expected_interval_;
   double latency_mean_;
   double latency_var_;
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 8d8a10266fec7..85d8911795c72 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -92,7 +92,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
   std::vector<std::string> input_topic_names;
   std::vector<std::string> input_names_long;
   std::vector<std::string> input_names_short;
-  std::vector<double> input_expected_intervals;
+  std::vector<bool> input_is_spawn_enabled;
 
   if (selected_input_channels.empty()) {
     RCLCPP_ERROR(this->get_logger(), "No input topics are specified.");
@@ -106,10 +106,9 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     input_topic_names.push_back(input_topic_name);
 
     // required parameter, but can set a default value
-    const double default_expected_interval = 0.1;  // [s]
-    const double expected_interval = declare_parameter<double>(
-      "input_channels." + selected_input_channel + ".expected_interval", default_expected_interval);
-    input_expected_intervals.push_back(expected_interval);
+    const bool spawn_enabled = declare_parameter<bool>(
+      "input_channels." + selected_input_channel + ".can_spawn_new_tracker", true);
+    input_is_spawn_enabled.push_back(spawn_enabled);
 
     // optional parameters
     const std::string default_name = selected_input_channel;
@@ -130,7 +129,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     input_channels_[i].input_topic = input_topic_names[i];
     input_channels_[i].long_name = input_names_long[i];
     input_channels_[i].short_name = input_names_short[i];
-    input_channels_[i].expected_interval = input_expected_intervals[i];
+    input_channels_[i].is_spawn_enabled = input_is_spawn_enabled[i];
   }
 
   // Initialize input manager
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index bf066e23828f8..6de48acdc1a84 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -31,15 +31,14 @@ void InputStream::init(const InputChannel & input_channel)
   input_topic_ = input_channel.input_topic;
   long_name_ = input_channel.long_name;
   short_name_ = input_channel.short_name;
-  expected_interval_ = input_channel.expected_interval;  // [s]
 
   // Initialize queue
   objects_que_.clear();
 
   // Initialize latency statistics
-  latency_mean_ = input_channel.expected_latency;  // [s]
+  latency_mean_ = 0.2;  // [s] (initial value)
   latency_var_ = 0.0;
-  interval_mean_ = expected_interval_;  // [s] (initial value)
+  interval_mean_ = 0.0;  // [s] (initial value)
   interval_var_ = 0.0;
 
   latest_measurement_time_ = node_.now();
@@ -49,7 +48,7 @@ void InputStream::init(const InputChannel & input_channel)
 bool InputStream::getTimestamps(
   rclcpp::Time & latest_measurement_time, rclcpp::Time & latest_message_time) const
 {
-  if (!is_time_initialized_) {
+  if (!isTimeInitialized()) {
     return false;
   }
   latest_measurement_time = latest_measurement_time_;
@@ -79,21 +78,38 @@ void InputStream::onMessage(
 
 void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time)
 {
-  // Filter parameters
-  constexpr double gain = 0.05;
+  // Update latency statistics
+  // skip initial messages for the latency statistics
+  if (initial_count_ > 4) {
+    const double latency = (now - objects_time).seconds();
+    if (initial_count_ < 16) {
+      // set higher gain for the initial messages
+      constexpr double initial_gain = 0.5;
+      latency_mean_ = (1.0 - initial_gain) * latency_mean_ + initial_gain * latency;
+    } else {
+      constexpr double gain = 0.05;
+      latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency;
+      const double latency_delta = latency - latency_mean_;
+      latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta;
+    }
+  }
 
   // Calculate interval, Update interval statistics
-  if (is_time_initialized_) {
+  if (initial_count_ > 4) {
     const double interval = (now - latest_message_time_).seconds();
-    // Check if the interval is regular
-    // The interval is considered regular if it is within 0.5 and 1.5 times the expected interval
-    bool is_interval_regular =
-      interval > 0.5 * expected_interval_ && interval < 1.5 * expected_interval_;
-
-    if (is_interval_regular) {
-      interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval;
-      const double interval_delta = interval - interval_mean_;
-      interval_var_ = (1.0 - gain) * interval_var_ + gain * interval_delta * interval_delta;
+    if (initial_count_ < 24) {
+      // Initialization
+      constexpr double initial_gain = 0.5;
+      interval_mean_ = (1.0 - initial_gain) * interval_mean_ + initial_gain * interval;
+    } else {
+      // The interval is considered regular if it is within 0.5 and 1.5 times the mean interval
+      bool update_statistics = interval > 0.5 * interval_mean_ && interval < 1.5 * interval_mean_;
+      if (update_statistics) {
+        constexpr double gain = 0.05;
+        interval_mean_ = (1.0 - gain) * interval_mean_ + gain * interval;
+        const double interval_delta = interval - interval_mean_;
+        interval_var_ = (1.0 - gain) * interval_var_ + gain * interval_delta * interval_delta;
+      }
     }
   }
 
@@ -101,13 +117,11 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim
   latest_message_time_ = now;
   latest_measurement_time_ =
     latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_;
-  if (!is_time_initialized_) is_time_initialized_ = true;
 
-  // Update latency statistics
-  const double latency = (latest_message_time_ - objects_time).seconds();
-  latency_mean_ = (1.0 - gain) * latency_mean_ + gain * latency;
-  const double latency_delta = latency - latency_mean_;
-  latency_var_ = (1.0 - gain) * latency_var_ + gain * latency_delta * latency_delta;
+  // Update the initial count, count only first 32 messages
+  if (initial_count_ < 32) {
+    initial_count_++;
+  }
 }
 
 void InputStream::getObjectsOlderThan(
@@ -195,17 +209,6 @@ void InputManager::getObjectTimeInterval(
   if (input_streams_.at(target_stream_idx_)->isTimeInitialized()) {
     rclcpp::Time latest_measurement_time =
       input_streams_.at(target_stream_idx_)->getLatestMeasurementTime();
-    // if the object_latest_time is newer than the next expected message time, set it older
-    // than the next expected message time
-    rclcpp::Time next_expected_message_time =
-      latest_measurement_time +
-      rclcpp::Duration::from_seconds(
-        target_stream_interval_ -
-        1.0 *
-          target_stream_interval_std_);  // next expected message time with 1 sigma safety margin
-    object_latest_time = object_latest_time > next_expected_message_time
-                           ? next_expected_message_time
-                           : object_latest_time;
 
     // if the object_latest_time is older than the latest measurement time, set it as the latest
     // object time

From 0c9c2d8b8d78bc7747247b08afbf8408cc1ffd25 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 13 May 2024 14:34:15 +0900
Subject: [PATCH 49/67] feat: implement spawn switch

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker/processor/input_manager.hpp   |  7 +++++++
 .../src/multi_object_tracker_core.cpp                  |  4 +++-
 .../src/processor/input_manager.cpp                    | 10 +++++++++-
 3 files changed, 19 insertions(+), 2 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index 03465cb56adae..d550547ea4278 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -65,6 +65,8 @@ class InputStream
     long_name = long_name_;
     short_name = short_name_;
   }
+  bool isSpawnEnabled() const { return is_spawn_enabled_; }
+
   std::string getLongName() const { return long_name_; }
   size_t getObjectsCount() const { return objects_que_.size(); }
   void getTimeStatistics(
@@ -92,6 +94,7 @@ class InputStream
   std::string input_topic_;
   std::string long_name_;
   std::string short_name_;
+  bool is_spawn_enabled_;
 
   size_t que_size_{30};
   std::deque<DetectedObjects> objects_que_;
@@ -125,6 +128,10 @@ class InputManager
     rclcpp::Time & object_oldest_time) const;
   void optimizeTimings();
   bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list);
+  bool isChannelSpawnEnabled(const uint & index) const
+  {
+    return input_streams_[index]->isSpawnEnabled();
+  }
 
 private:
   rclcpp::Node & node_;
diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 85d8911795c72..94512ed0691f4 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -342,7 +342,9 @@ void MultiObjectTracker::runProcess(
   processor_->prune(measurement_time);
 
   /* spawn new tracker */
-  processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index);
+  if (input_manager_->isChannelSpawnEnabled(channel_index)) {
+    processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index);
+  }
 }
 
 void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time)
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 6de48acdc1a84..e3eb9f7c33cd1 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -31,6 +31,7 @@ void InputStream::init(const InputChannel & input_channel)
   input_topic_ = input_channel.input_topic;
   long_name_ = input_channel.long_name;
   short_name_ = input_channel.short_name;
+  is_spawn_enabled_ = input_channel.is_spawn_enabled;
 
   // Initialize queue
   objects_que_.clear();
@@ -166,8 +167,9 @@ void InputManager::init(const std::vector<InputChannel> & input_channels)
     return;
   }
 
+  // Initialize input streams
   sub_objects_array_.resize(input_size_);
-
+  bool is_any_spawn_enabled = false;
   for (size_t i = 0; i < input_size_; i++) {
     uint index(i);
     InputStream input_stream(node_, index);
@@ -175,6 +177,7 @@ void InputManager::init(const std::vector<InputChannel> & input_channels)
     input_stream.setTriggerFunction(
       std::bind(&InputManager::onTrigger, this, std::placeholders::_1));
     input_streams_.push_back(std::make_shared<InputStream>(input_stream));
+    is_any_spawn_enabled |= input_streams_.at(i)->isSpawnEnabled();
 
     // Set subscription
     RCLCPP_INFO(
@@ -186,6 +189,11 @@ void InputManager::init(const std::vector<InputChannel> & input_channels)
       input_channels[i].input_topic, rclcpp::QoS{1}, func);
   }
 
+  // Check if any spawn enabled input streams
+  if (!is_any_spawn_enabled) {
+    RCLCPP_ERROR(node_.get_logger(), "InputManager::init No spawn enabled input streams");
+    return;
+  }
   is_initialized_ = true;
 }
 

From 204595745f9600c0ba6ddcce9a778bd8fccdabce Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 13 May 2024 15:45:34 +0900
Subject: [PATCH 50/67] fix: remove debug messages

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../src/multi_object_tracker_core.cpp         | 38 -------------------
 .../src/processor/input_manager.cpp           | 14 -------
 2 files changed, 52 deletions(-)

diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 94512ed0691f4..c3bf7023fb068 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -255,44 +255,6 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
   }
   // process end
   debugger_->endMeasurementTime(this->now());
-
-  /* DEBUG */
-  const rclcpp::Time latest_time(objects_list.back().second.header.stamp);
-  RCLCPP_INFO(
-    this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f",
-    (current_time - latest_time).seconds(), (current_time - oldest_time).seconds());
-
-  // count objects on each channel
-  std::vector<int> object_counts;
-  object_counts.resize(input_channel_size_);
-  // initialize to zero
-  for (size_t i = 0; i < input_channel_size_; i++) {
-    object_counts[i] = 0;
-  }
-  for (const auto & objects_data : objects_list) {
-    object_counts[objects_data.first] += 1;
-  }
-  // print result
-  std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: ";
-  for (size_t i = 0; i < input_channel_size_; i++) {
-    object_counts_str +=
-      input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + "  ";
-  }
-  RCLCPP_INFO(this->get_logger(), object_counts_str.c_str());
-
-  std::string object_info_str = "";
-  object_info_str += "MultiObjectTracker::onMessage Objects channel and time: \n";
-  for (const auto & objects_data : objects_list) {
-    const auto & objects = objects_data.second;
-    const auto & channel_index = objects_data.first;
-    const auto & time = rclcpp::Time(objects.header.stamp);
-    // print object information
-    object_info_str += input_channels_[channel_index].long_name + " " +
-                       std::to_string((current_time - time).seconds()) + " \n";
-  }
-  RCLCPP_INFO(this->get_logger(), object_info_str.c_str());
-
-  /* DEBUG END */
 }
 
 void MultiObjectTracker::runProcess(
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index e3eb9f7c33cd1..d6f1619d9dd73 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -253,20 +253,6 @@ void InputManager::optimizeTimings()
         selected_stream_interval = interval_mean;
         selected_stream_interval_std = std::sqrt(interval_var);
       }
-
-      /* DEBUG */
-      std::string long_name, short_name;
-      rclcpp::Time latest_measurement_time, latest_message_time;
-      input_stream->getNames(long_name, short_name);
-      input_stream->getTimestamps(latest_measurement_time, latest_message_time);
-      double latency_message = (node_.now() - latest_message_time).seconds();
-      double latency_measurement = (node_.now() - latest_measurement_time).seconds();
-      RCLCPP_INFO(
-        node_.get_logger(),
-        "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: "
-        "%f, std: %f, latest measurement latency: %f, latest message latency: %f",
-        long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean,
-        std::sqrt(interval_var), latency_measurement, latency_message);
     }
   }
 

From eb09e03efaa587bf7dcad4d6a550eda0a332588f Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 13 May 2024 16:39:33 +0900
Subject: [PATCH 51/67] chore: update readme

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 perception/multi_object_tracker/README.md | 43 ++++++++++++-----------
 1 file changed, 23 insertions(+), 20 deletions(-)

diff --git a/perception/multi_object_tracker/README.md b/perception/multi_object_tracker/README.md
index 311fe7b6da716..11302612cd046 100644
--- a/perception/multi_object_tracker/README.md
+++ b/perception/multi_object_tracker/README.md
@@ -46,27 +46,34 @@ 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
 
@@ -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

From dab7cdc88e76278a7a548cc4c2a8dad18dd59942 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 13 May 2024 16:46:27 +0900
Subject: [PATCH 52/67] fix: change tentative object topic

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 perception/multi_object_tracker/src/debugger/debugger.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp
index 0334eca58de3a..ad4ce4afad498 100644
--- a/perception/multi_object_tracker/src/debugger/debugger.cpp
+++ b/perception/multi_object_tracker/src/debugger/debugger.cpp
@@ -30,7 +30,7 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_
   if (debug_settings_.publish_tentative_objects) {
     debug_tentative_objects_pub_ =
       node_.create_publisher<autoware_auto_perception_msgs::msg::TrackedObjects>(
-        "debug/tentative_objects", rclcpp::QoS{1});
+        "multi_object_tracker/debug/tentative_objects", rclcpp::QoS{1});
   }
 
   if (debug_settings_.publish_debug_markers) {

From 7a0553e4364444fe88aa47a9a9c08417675245a5 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 13 May 2024 18:17:10 +0900
Subject: [PATCH 53/67] Revert "fix: remove debug messages"

This reverts commit 725a49ee6c382f73b54fe50bf9077aca6049e199.

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../src/multi_object_tracker_core.cpp         | 38 +++++++++++++++++++
 .../src/processor/input_manager.cpp           | 14 +++++++
 2 files changed, 52 insertions(+)

diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index c3bf7023fb068..94512ed0691f4 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -255,6 +255,44 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
   }
   // process end
   debugger_->endMeasurementTime(this->now());
+
+  /* DEBUG */
+  const rclcpp::Time latest_time(objects_list.back().second.header.stamp);
+  RCLCPP_INFO(
+    this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f",
+    (current_time - latest_time).seconds(), (current_time - oldest_time).seconds());
+
+  // count objects on each channel
+  std::vector<int> object_counts;
+  object_counts.resize(input_channel_size_);
+  // initialize to zero
+  for (size_t i = 0; i < input_channel_size_; i++) {
+    object_counts[i] = 0;
+  }
+  for (const auto & objects_data : objects_list) {
+    object_counts[objects_data.first] += 1;
+  }
+  // print result
+  std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: ";
+  for (size_t i = 0; i < input_channel_size_; i++) {
+    object_counts_str +=
+      input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + "  ";
+  }
+  RCLCPP_INFO(this->get_logger(), object_counts_str.c_str());
+
+  std::string object_info_str = "";
+  object_info_str += "MultiObjectTracker::onMessage Objects channel and time: \n";
+  for (const auto & objects_data : objects_list) {
+    const auto & objects = objects_data.second;
+    const auto & channel_index = objects_data.first;
+    const auto & time = rclcpp::Time(objects.header.stamp);
+    // print object information
+    object_info_str += input_channels_[channel_index].long_name + " " +
+                       std::to_string((current_time - time).seconds()) + " \n";
+  }
+  RCLCPP_INFO(this->get_logger(), object_info_str.c_str());
+
+  /* DEBUG END */
 }
 
 void MultiObjectTracker::runProcess(
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index d6f1619d9dd73..e3eb9f7c33cd1 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -253,6 +253,20 @@ void InputManager::optimizeTimings()
         selected_stream_interval = interval_mean;
         selected_stream_interval_std = std::sqrt(interval_var);
       }
+
+      /* DEBUG */
+      std::string long_name, short_name;
+      rclcpp::Time latest_measurement_time, latest_message_time;
+      input_stream->getNames(long_name, short_name);
+      input_stream->getTimestamps(latest_measurement_time, latest_message_time);
+      double latency_message = (node_.now() - latest_message_time).seconds();
+      double latency_measurement = (node_.now() - latest_measurement_time).seconds();
+      RCLCPP_INFO(
+        node_.get_logger(),
+        "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: "
+        "%f, std: %f, latest measurement latency: %f, latest message latency: %f",
+        long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean,
+        std::sqrt(interval_var), latency_measurement, latency_message);
     }
   }
 

From 094110f1bc58d652928c4f7e313b232e1ff1e350 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Mon, 13 May 2024 18:45:56 +0900
Subject: [PATCH 54/67] fix: reset times when jumps to past

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../src/processor/input_manager.cpp           | 34 ++++++++++++++++---
 1 file changed, 30 insertions(+), 4 deletions(-)

diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index e3eb9f7c33cd1..0bb399a02b0c2 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -116,8 +116,18 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim
 
   // Update time
   latest_message_time_ = now;
-  latest_measurement_time_ =
-    latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_;
+  if (std::abs((latest_measurement_time_ - objects_time).seconds()) > 3.0) {
+    // Reset the latest measurement time if the time difference is too large
+    latest_measurement_time_ = objects_time;
+    RCLCPP_WARN(
+      node_.get_logger(),
+      "InputManager::updateTimingStatus %s: Resetting the latest measurement time to %f",
+      long_name_.c_str(), objects_time.seconds());
+  } else {
+    // Aware reversed message arrival
+    latest_measurement_time_ =
+      latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_;
+  }
 
   // Update the initial count, count only first 32 messages
   if (initial_count_ < 32) {
@@ -314,11 +324,27 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
     });
 
   // Update the latest object time
-  if (!objects_list.empty()) {
+  bool is_any_object = !objects_list.empty();
+  if (is_any_object) {
     latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp);
+  } else {
+    // check time jump
+    if ((now - latest_object_time_).seconds() < 0.0) {
+      RCLCPP_WARN(
+        node_.get_logger(),
+        "InputManager::getObjects Time jump detected, now: %f, latest_object_time_: %f",
+        now.seconds(), latest_object_time_.seconds());
+      latest_object_time_ =
+        now - rclcpp::Duration::from_seconds(3.0);  // reset the latest object time to 3 seconds ago
+    } else {
+      RCLCPP_WARN(
+        node_.get_logger(),
+        "InputManager::getObjects No objects in the object list, object time band from %f to %f",
+        (now - object_oldest_time).seconds(), (now - object_latest_time).seconds());
+    }
   }
 
-  return !objects_list.empty();
+  return is_any_object;
 }
 
 }  // namespace multi_object_tracker

From c1a85b8161add9d384b3437559d9ff3289c6e4eb Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Tue, 14 May 2024 15:02:56 +0900
Subject: [PATCH 55/67] fix: check if interval is negative

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker/src/processor/input_manager.cpp  | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 0bb399a02b0c2..1e5bee9887063 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -98,7 +98,13 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim
   // Calculate interval, Update interval statistics
   if (initial_count_ > 4) {
     const double interval = (now - latest_message_time_).seconds();
-    if (initial_count_ < 24) {
+    if (interval < 0.0) {
+      RCLCPP_WARN(
+        node_.get_logger(),
+        "InputManager::updateTimingStatus %s: Negative interval detected, now: %f, "
+        "latest_message_time_: %f",
+        long_name_.c_str(), now.seconds(), latest_message_time_.seconds());
+    } else if (initial_count_ < 24) {
       // Initialization
       constexpr double initial_gain = 0.5;
       interval_mean_ = (1.0 - initial_gain) * interval_mean_ + initial_gain * interval;

From c12109a2933cb0c1a5ac00832a38005417e02c9f Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Tue, 14 May 2024 15:16:51 +0900
Subject: [PATCH 56/67] fix: missing config, default value

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker/config/input_channels.param.yaml      | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/perception/multi_object_tracker/config/input_channels.param.yaml b/perception/multi_object_tracker/config/input_channels.param.yaml
index 20d717ee36e1f..b57f37675d4f1 100644
--- a/perception/multi_object_tracker/config/input_channels.param.yaml
+++ b/perception/multi_object_tracker/config/input_channels.param.yaml
@@ -49,13 +49,14 @@
           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: false
+        can_spawn_new_tracker: true
         optional:
           name: "camera_lidar_fusion"
           short_name: "CLf"

From c589c3a7e424e1202ba0a6f0f39e505ed9bf7274 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Tue, 14 May 2024 15:21:01 +0900
Subject: [PATCH 57/67] fix: remove debug messages

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../src/multi_object_tracker_core.cpp         | 38 -------------------
 .../src/processor/input_manager.cpp           | 14 -------
 2 files changed, 52 deletions(-)

diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
index 94512ed0691f4..c3bf7023fb068 100644
--- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
+++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp
@@ -255,44 +255,6 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
   }
   // process end
   debugger_->endMeasurementTime(this->now());
-
-  /* DEBUG */
-  const rclcpp::Time latest_time(objects_list.back().second.header.stamp);
-  RCLCPP_INFO(
-    this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f",
-    (current_time - latest_time).seconds(), (current_time - oldest_time).seconds());
-
-  // count objects on each channel
-  std::vector<int> object_counts;
-  object_counts.resize(input_channel_size_);
-  // initialize to zero
-  for (size_t i = 0; i < input_channel_size_; i++) {
-    object_counts[i] = 0;
-  }
-  for (const auto & objects_data : objects_list) {
-    object_counts[objects_data.first] += 1;
-  }
-  // print result
-  std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: ";
-  for (size_t i = 0; i < input_channel_size_; i++) {
-    object_counts_str +=
-      input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + "  ";
-  }
-  RCLCPP_INFO(this->get_logger(), object_counts_str.c_str());
-
-  std::string object_info_str = "";
-  object_info_str += "MultiObjectTracker::onMessage Objects channel and time: \n";
-  for (const auto & objects_data : objects_list) {
-    const auto & objects = objects_data.second;
-    const auto & channel_index = objects_data.first;
-    const auto & time = rclcpp::Time(objects.header.stamp);
-    // print object information
-    object_info_str += input_channels_[channel_index].long_name + " " +
-                       std::to_string((current_time - time).seconds()) + " \n";
-  }
-  RCLCPP_INFO(this->get_logger(), object_info_str.c_str());
-
-  /* DEBUG END */
 }
 
 void MultiObjectTracker::runProcess(
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 1e5bee9887063..5d4fefd69b03e 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -269,20 +269,6 @@ void InputManager::optimizeTimings()
         selected_stream_interval = interval_mean;
         selected_stream_interval_std = std::sqrt(interval_var);
       }
-
-      /* DEBUG */
-      std::string long_name, short_name;
-      rclcpp::Time latest_measurement_time, latest_message_time;
-      input_stream->getNames(long_name, short_name);
-      input_stream->getTimestamps(latest_measurement_time, latest_message_time);
-      double latency_message = (node_.now() - latest_message_time).seconds();
-      double latency_measurement = (node_.now() - latest_measurement_time).seconds();
-      RCLCPP_INFO(
-        node_.get_logger(),
-        "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: "
-        "%f, std: %f, latest measurement latency: %f, latest message latency: %f",
-        long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean,
-        std::sqrt(interval_var), latency_measurement, latency_message);
     }
   }
 

From 48a8861df8de788a379686a12672ead65c2b623a Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Tue, 14 May 2024 15:54:07 +0900
Subject: [PATCH 58/67] fix: change no-object message level

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker/src/processor/input_manager.cpp      | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 5d4fefd69b03e..dfaae6252745c 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -321,7 +321,7 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
     latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp);
   } else {
     // check time jump
-    if ((now - latest_object_time_).seconds() < 0.0) {
+    if (now < latest_object_time_) {
       RCLCPP_WARN(
         node_.get_logger(),
         "InputManager::getObjects Time jump detected, now: %f, latest_object_time_: %f",
@@ -329,7 +329,7 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
       latest_object_time_ =
         now - rclcpp::Duration::from_seconds(3.0);  // reset the latest object time to 3 seconds ago
     } else {
-      RCLCPP_WARN(
+      RCLCPP_DEBUG(
         node_.get_logger(),
         "InputManager::getObjects No objects in the object list, object time band from %f to %f",
         (now - object_oldest_time).seconds(), (now - object_latest_time).seconds());

From f873ab8d2efdd12565f783aa2a330fe41d425fb4 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <technolojin@gmail.com>
Date: Thu, 16 May 2024 16:37:20 +0900
Subject: [PATCH 59/67] Update
 perception/multi_object_tracker/include/multi_object_tracker/debugger/debug_object.hpp

Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com>
Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../include/multi_object_tracker/debugger/debug_object.hpp      | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

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
index c312edc843259..8138d388d1901 100644
--- 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
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// 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.

From 1ba2c79ef839a71afb5844883283eeb8cf8f8568 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 16 May 2024 16:46:47 +0900
Subject: [PATCH 60/67] chore: Update copyright to uppercase

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../include/multi_object_tracker/debugger/debugger.hpp          | 2 +-
 .../include/multi_object_tracker/processor/input_manager.hpp    | 2 +-
 .../include/multi_object_tracker/processor/processor.hpp        | 2 +-
 perception/multi_object_tracker/src/debugger/debug_object.cpp   | 2 +-
 perception/multi_object_tracker/src/debugger/debugger.cpp       | 2 +-
 perception/multi_object_tracker/src/processor/input_manager.cpp | 2 +-
 perception/multi_object_tracker/src/processor/processor.cpp     | 2 +-
 7 files changed, 7 insertions(+), 7 deletions(-)

diff --git a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
index 8edfc102b56db..1c632f5bebba0 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/debugger/debugger.hpp
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// 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.
diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
index d550547ea4278..ad8c4ae6e5e24 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// 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.
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
index de0fe0854254a..310871dfcb07a 100644
--- a/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp
+++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/processor.hpp
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// 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.
diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp
index 11ecafba19e53..65e2068ab04ac 100644
--- a/perception/multi_object_tracker/src/debugger/debug_object.cpp
+++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// 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.
diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp
index ad4ce4afad498..2baa970850a18 100644
--- a/perception/multi_object_tracker/src/debugger/debugger.cpp
+++ b/perception/multi_object_tracker/src/debugger/debugger.cpp
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// 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.
diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index dfaae6252745c..aada2c0403222 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// 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.
diff --git a/perception/multi_object_tracker/src/processor/processor.cpp b/perception/multi_object_tracker/src/processor/processor.cpp
index a5a1f18a64ca0..08df50fa66993 100644
--- a/perception/multi_object_tracker/src/processor/processor.cpp
+++ b/perception/multi_object_tracker/src/processor/processor.cpp
@@ -1,4 +1,4 @@
-// Copyright 2024 Tier IV, Inc.
+// 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.

From 9edd2e06913fcb4200ed031b7e91434162adb556 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 16 May 2024 16:53:16 +0900
Subject: [PATCH 61/67] chore: fix readme links to config files

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 perception/multi_object_tracker/README.md | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/perception/multi_object_tracker/README.md b/perception/multi_object_tracker/README.md
index 11302612cd046..ad9e43355aebf 100644
--- a/perception/multi_object_tracker/README.md
+++ b/perception/multi_object_tracker/README.md
@@ -77,7 +77,7 @@ Available input channels are defined in [input_channels.param.yaml](config/input
 
 ### 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
 

From cf6cf3c407249be66c38f69a5a178fd1ad28bd8a Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 17 May 2024 10:21:40 +0900
Subject: [PATCH 62/67] chore: move and rename uuid functions

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../debugger/debug_object.hpp                 | 20 ------------
 .../src/debugger/debug_object.cpp             | 31 +++++++++++++++++--
 2 files changed, 28 insertions(+), 23 deletions(-)

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
index 8138d388d1901..6caa69181dd9d 100644
--- 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
@@ -97,26 +97,6 @@ class TrackerObjectDebugger
     visualization_msgs::msg::MarkerArray & marker_array) const;
   void process();
   void getMessage(visualization_msgs::msg::MarkerArray & marker_array) const;
-
-private:
-  std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & u) const
-  {
-    std::stringstream ss;
-    for (auto i = 0; i < 16; ++i) {
-      ss << std::hex << std::setfill('0') << std::setw(2) << +u.uuid[i];
-    }
-    return ss.str();
-  }
-
-  boost::uuids::uuid to_boost_uuid(const unique_identifier_msgs::msg::UUID & uuid_msg)
-  {
-    const std::string uuid_str = uuid_to_string(uuid_msg);
-    boost::uuids::string_generator gen;
-    boost::uuids::uuid uuid = gen(uuid_str);
-    return uuid;
-  }
-
-  int32_t uuid_to_int(const boost::uuids::uuid & uuid) { return boost::uuids::hash_value(uuid); }
 };
 
 #endif  // MULTI_OBJECT_TRACKER__DEBUGGER__DEBUG_OBJECT_HPP_
diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp
index 65e2068ab04ac..ebbeab43a155b 100644
--- a/perception/multi_object_tracker/src/debugger/debug_object.cpp
+++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp
@@ -23,6 +23,31 @@
 #include <sstream>
 #include <string>
 
+namespace
+{
+std::string uuidToString(const unique_identifier_msgs::msg::UUID & uuid_msg)
+{
+  std::stringstream ss;
+  for (auto i = 0; i < 16; ++i) {
+    ss << std::hex << std::setfill('0') << std::setw(2) << +uuid_msg.uuid[i];
+  }
+  return ss.str();
+}
+
+boost::uuids::uuid uuidToBoostUuid(const unique_identifier_msgs::msg::UUID & uuid_msg)
+{
+  const std::string uuid_str = uuidToString(uuid_msg);
+  boost::uuids::string_generator gen;
+  boost::uuids::uuid uuid = gen(uuid_str);
+  return uuid;
+}
+
+int32_t uuidToInt(const boost::uuids::uuid & uuid)
+{
+  return boost::uuids::hash_value(uuid);
+}
+}  // namespace
+
 TrackerObjectDebugger::TrackerObjectDebugger(std::string frame_id)
 {
   // set frame id
@@ -67,9 +92,9 @@ void TrackerObjectDebugger::collect(
 
     autoware_auto_perception_msgs::msg::TrackedObject tracked_object;
     (*(tracker_itr))->getTrackedObject(message_time, tracked_object);
-    object_data.uuid = to_boost_uuid(tracked_object.object_id);
-    object_data.uuid_int = uuid_to_int(object_data.uuid);
-    object_data.uuid_str = uuid_to_string(tracked_object.object_id);
+    object_data.uuid = uuidToBoostUuid(tracked_object.object_id);
+    object_data.uuid_int = uuidToInt(object_data.uuid);
+    object_data.uuid_str = uuidToString(tracked_object.object_id);
 
     // tracker
     bool is_associated = false;

From d5efe1017eec1bb7cfba027d22a8d0e1fdce8f79 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 17 May 2024 10:22:20 +0900
Subject: [PATCH 63/67] chore: fix debug topic to use node name

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 perception/multi_object_tracker/src/debugger/debugger.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/perception/multi_object_tracker/src/debugger/debugger.cpp b/perception/multi_object_tracker/src/debugger/debugger.cpp
index 2baa970850a18..afb0ecdc22996 100644
--- a/perception/multi_object_tracker/src/debugger/debugger.cpp
+++ b/perception/multi_object_tracker/src/debugger/debugger.cpp
@@ -30,12 +30,12 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node, const std::string & frame_
   if (debug_settings_.publish_tentative_objects) {
     debug_tentative_objects_pub_ =
       node_.create_publisher<autoware_auto_perception_msgs::msg::TrackedObjects>(
-        "multi_object_tracker/debug/tentative_objects", rclcpp::QoS{1});
+        "~/debug/tentative_objects", rclcpp::QoS{1});
   }
 
   if (debug_settings_.publish_debug_markers) {
     debug_objects_markers_pub_ = node_.create_publisher<visualization_msgs::msg::MarkerArray>(
-      "multi_object_tracker/debug/objects_markers", rclcpp::QoS{1});
+      "~/debug/objects_markers", rclcpp::QoS{1});
   }
 
   // initialize timestamps

From 4445c45fb9bcd9159683a24afa0cf7d259aa45b2 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Fri, 17 May 2024 10:22:46 +0900
Subject: [PATCH 64/67] chore: express meaning of threshold

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../multi_object_tracker/src/processor/input_manager.cpp       | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index aada2c0403222..44fa515061d2a 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -122,7 +122,8 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim
 
   // Update time
   latest_message_time_ = now;
-  if (std::abs((latest_measurement_time_ - objects_time).seconds()) > 3.0) {
+  constexpr double delay_threshold = 3.0;  // [s]
+  if (std::abs((latest_measurement_time_ - objects_time).seconds()) > delay_threshold) {
     // Reset the latest measurement time if the time difference is too large
     latest_measurement_time_ = objects_time;
     RCLCPP_WARN(

From f218f84a943278b28f2ac55c0cf8f142b108e376 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 23 May 2024 09:34:36 +0900
Subject: [PATCH 65/67] feat: revise decay rate, update function

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../src/tracker/model/tracker_base.cpp        | 45 ++++++++++++-------
 1 file changed, 30 insertions(+), 15 deletions(-)

diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
index b62cf05449603..4318da0569721 100644
--- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
+++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp
@@ -21,6 +21,14 @@
 #include <algorithm>
 #include <random>
 
+namespace
+{
+float updateProbability(float prior, float true_positive, float false_positive)
+{
+  return (prior * true_positive) / (prior * true_positive + (1 - prior) * false_positive);
+}
+}  // namespace
+
 Tracker::Tracker(
   const rclcpp::Time & time,
   const std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> & classification,
@@ -43,7 +51,13 @@ Tracker::Tracker(
 void Tracker::initializeExistenceProbabilities(
   const uint & channel_index, const float & existence_probability)
 {
-  existence_probabilities_[channel_index] = 0.8 + 0.2 * existence_probability;
+  // The initial existence probability is modeled
+  // since the incoming object's existence probability is not reliable
+  // existence probability on each channel
+  constexpr float initial_existence_probability = 0.1;
+  existence_probabilities_[channel_index] = initial_existence_probability;
+
+  // total existence probability
   total_existence_probability_ = existence_probability;
 }
 
@@ -54,30 +68,31 @@ bool Tracker::updateWithMeasurement(
 {
   // Update existence probability
   {
-    float existence_probability_from_object = object.existence_probability;
     no_measurement_count_ = 0;
     ++total_measurement_count_;
 
     // existence probability on each channel
     const double delta_time = (measurement_time - last_update_with_measurement_time_).seconds();
-    const double decay_rate = 5.0 / 10.0;
-
-    const float gain = 0.4;
-    const float probability_detected = 0.99;
-    // existence_probabilities_[channel_index] = existence_probability_from_object;
-    existence_probabilities_[channel_index] =
-      gain * probability_detected + (1 - gain) * existence_probabilities_[channel_index];
-
+    const double decay_rate = -log(0.5) / 0.3;  // 50% decay in 0.3s
+    constexpr float probability_true_detection = 0.9;
+    constexpr float probability_false_detection = 0.2;
+
+    // update measured channel probability
+    existence_probabilities_[channel_index] = updateProbability(
+      existence_probabilities_[channel_index], probability_true_detection,
+      probability_false_detection);
+    // decay other channel probabilities
     for (size_t i = 0; i < existence_probabilities_.size(); ++i) {
       if (i == channel_index) {
         continue;
       }
-      existence_probabilities_[i] *= std::exp(-decay_rate * delta_time);
+      existence_probabilities_[i] *= std::exp(decay_rate * delta_time);
     }
 
-    // total existence probability - object is detected
-    total_existence_probability_ +=
-      (1 - total_existence_probability_) * existence_probability_from_object;
+    // update total existence probability
+    const float & existence_probability_from_object = object.existence_probability;
+    total_existence_probability_ = updateProbability(
+      total_existence_probability_, existence_probability_from_object, probability_false_detection);
   }
 
   last_update_with_measurement_time_ = measurement_time;
@@ -96,7 +111,7 @@ bool Tracker::updateWithoutMeasurement(const rclcpp::Time & now)
   {
     // decay existence probability
     double const delta_time = (now - last_update_with_measurement_time_).seconds();
-    double const decay_rate = 5.0 / 10.0;
+    const double decay_rate = -log(0.5) / 0.3;  // 50% decay in 0.3s
     for (size_t i = 0; i < existence_probabilities_.size(); ++i) {
       existence_probabilities_[i] *= std::exp(-decay_rate * delta_time);
     }

From b62399378f67d07e949cd6174a33146616a53bb2 Mon Sep 17 00:00:00 2001
From: Taekjin LEE <taekjin.lee@tier4.jp>
Date: Thu, 23 May 2024 12:14:47 +0900
Subject: [PATCH 66/67] fix: define constants with explanation

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
---
 .../src/processor/input_manager.cpp              | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 44fa515061d2a..6fd6325a77b61 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -79,11 +79,15 @@ void InputStream::onMessage(
 
 void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time)
 {
+  // Define constants
+  constexpr int SKIP_COUNT = 4; // Skip the initial messages
+  constexpr int INITIALIZATION_COUNT = 16; // Initialization process count
+
   // Update latency statistics
   // skip initial messages for the latency statistics
-  if (initial_count_ > 4) {
+  if (initial_count_ > SKIP_COUNT) {
     const double latency = (now - objects_time).seconds();
-    if (initial_count_ < 16) {
+    if (initial_count_ < INITIALIZATION_COUNT) {
       // set higher gain for the initial messages
       constexpr double initial_gain = 0.5;
       latency_mean_ = (1.0 - initial_gain) * latency_mean_ + initial_gain * latency;
@@ -96,7 +100,7 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim
   }
 
   // Calculate interval, Update interval statistics
-  if (initial_count_ > 4) {
+  if (initial_count_ > SKIP_COUNT) {
     const double interval = (now - latest_message_time_).seconds();
     if (interval < 0.0) {
       RCLCPP_WARN(
@@ -104,7 +108,7 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim
         "InputManager::updateTimingStatus %s: Negative interval detected, now: %f, "
         "latest_message_time_: %f",
         long_name_.c_str(), now.seconds(), latest_message_time_.seconds());
-    } else if (initial_count_ < 24) {
+    } else if (initial_count_ < INITIALIZATION_COUNT) {
       // Initialization
       constexpr double initial_gain = 0.5;
       interval_mean_ = (1.0 - initial_gain) * interval_mean_ + initial_gain * interval;
@@ -136,8 +140,8 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim
       latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_;
   }
 
-  // Update the initial count, count only first 32 messages
-  if (initial_count_ < 32) {
+  // Update the initial count
+  if (initial_count_ < INITIALIZATION_COUNT) {
     initial_count_++;
   }
 }

From 13adc73da8c846d471f6c0e1a0580cfb4f6a6b10 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Thu, 23 May 2024 03:16:55 +0000
Subject: [PATCH 67/67] style(pre-commit): autofix

---
 .../multi_object_tracker/src/processor/input_manager.cpp      | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp
index 6fd6325a77b61..e29b54e971491 100644
--- a/perception/multi_object_tracker/src/processor/input_manager.cpp
+++ b/perception/multi_object_tracker/src/processor/input_manager.cpp
@@ -80,8 +80,8 @@ void InputStream::onMessage(
 void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Time & objects_time)
 {
   // Define constants
-  constexpr int SKIP_COUNT = 4; // Skip the initial messages
-  constexpr int INITIALIZATION_COUNT = 16; // Initialization process count
+  constexpr int SKIP_COUNT = 4;             // Skip the initial messages
+  constexpr int INITIALIZATION_COUNT = 16;  // Initialization process count
 
   // Update latency statistics
   // skip initial messages for the latency statistics