chore: add comments, refactoring

Signed-off-by: Taekjin LEE <[email protected]>

perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp

@@ -117,17 +117,13 @@ class InputManager
 {
 public:
   explicit InputManager(rclcpp::Node & node);
-
   void init(const std::vector<InputChannel> & input_channels);
-  void setTriggerFunction(std::function<void()> func_trigger) { func_trigger_ = func_trigger; }
 
+  void setTriggerFunction(std::function<void()> func_trigger) { func_trigger_ = func_trigger; }
   void onTrigger(const uint & index) const;
 
-  void getObjectTimeInterval(
-    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);
+
   bool isChannelSpawnEnabled(const uint & index) const
   {
     return input_streams_[index]->isSpawnEnabled();
@@ -151,6 +147,12 @@ class InputManager
   double target_stream_interval_std_{0.02};  // [s]
   double target_latency_{0.2};               // [s]
   double target_latency_band_{1.0};          // [s]
+
+private:
+  void getObjectTimeInterval(
+    const rclcpp::Time & now, rclcpp::Time & object_latest_time,
+    rclcpp::Time & object_oldest_time) const;
+  void optimizeTimings();
 };
 
 }  // namespace multi_object_tracker

perception/multi_object_tracker/src/processor/input_manager.cpp

@@ -127,15 +127,16 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim
   // Update time
   latest_message_time_ = now;
   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
+  if (objects_time < latest_measurement_time_ - rclcpp::Duration::from_seconds(delay_threshold)) {
+    // If the given object time is older than the latest measurement time by more than the
+    // threshold, the system time may have been reset. Reset the latest measurement time
     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
+    // Update only if the object time is newer than the latest measurement time
     latest_measurement_time_ =
       latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_;
   }
@@ -237,36 +238,36 @@ void InputManager::getObjectTimeInterval(
   const rclcpp::Time & now, rclcpp::Time & object_latest_time,
   rclcpp::Time & object_oldest_time) const
 {
+  // Set the object time interval
+
+  // 1. object_latest_time
+  // The object_latest_time is the current time minus the target stream latency
   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
+    now - rclcpp::Duration::from_seconds(target_stream_latency_ - 0.1 * target_stream_latency_std_);
+
   // 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 =
+    const rclcpp::Time latest_measurement_time =
       input_streams_.at(target_stream_idx_)->getLatestMeasurementTime();
-
     // if the object_latest_time is older than the latest measurement time, set it to the latest
     // object time
     object_latest_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 = object_oldest_time < latest_exported_object_time_
-                         ? latest_exported_object_time_
-                         : object_oldest_time;
-
-  // check the object time interval is valid
-  if (object_oldest_time > object_latest_time) {
-    RCLCPP_WARN(
-      node_.get_logger(),
-      "InputManager::getObjectTimeInterval Invalid object time interval, object_latest_time: %f, "
-      "object_oldest_time: %f",
-      (now - object_latest_time).seconds(), (now - object_oldest_time).seconds());
-    object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0);
+  // 2. object_oldest_time
+  // The default object_oldest_time is to have a 1-second time interval
+  const rclcpp::Time object_oldest_time_default =
+    object_latest_time - rclcpp::Duration::from_seconds(1.0);
+  if (latest_exported_object_time_ < object_oldest_time_default) {
+    // if the latest exported object time is too old, set to the default
+    object_oldest_time = object_oldest_time_default;
+  } else if (latest_exported_object_time_ > object_latest_time) {
+    // if the latest exported object time is newer than the object_latest_time, set to the default
+    object_oldest_time = object_oldest_time_default;
+  } else {
+    // The object_oldest_time is the latest exported object time
+    object_oldest_time = latest_exported_object_time_;
   }
 }
 
@@ -316,7 +317,8 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
   objects_list.clear();
 
   // Get the time interval for the objects
-  rclcpp::Time object_latest_time, object_oldest_time;
+  rclcpp::Time object_latest_time;
+  rclcpp::Time object_oldest_time;
   getObjectTimeInterval(now, object_latest_time, object_oldest_time);
 
   // Optimize the target stream, latency, and its band
@@ -343,16 +345,19 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
   if (is_any_object) {
     latest_exported_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp);
   } else {
-    // check time jump
+    // check time jump back
     if (now < latest_exported_object_time_) {
       RCLCPP_WARN(
         node_.get_logger(),
-        "InputManager::getObjects Time jump detected, now: %f, latest_exported_object_time_: %f",
+        "InputManager::getObjects Detected jump back in time, now: %f, "
+        "latest_exported_object_time_: %f",
         now.seconds(), latest_exported_object_time_.seconds());
-      latest_exported_object_time_ =
-        now - rclcpp::Duration::from_seconds(
-                3.0);  // reset the latest exported object time to 3 seconds ago
+      // reset the latest exported object time to 3 seconds ago,
+      const rclcpp::Time latest_exported_object_time_default =
+        now - rclcpp::Duration::from_seconds(3.0);
+      latest_exported_object_time_ = latest_exported_object_time_default;
     } else {
+      // No objects in the object list, no update for the latest exported object time
       RCLCPP_DEBUG(
         node_.get_logger(),
         "InputManager::getObjects No objects in the object list, object time band from %f to %f",