feat: revise object time range logic

Author: technolojin

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"

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

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

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)