feat: remove expected interval

Author: technolojin

perception/multi_object_tracker/config/input_channels.param.yaml

@@ -3,80 +3,79 @@
     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"
       # LIDAR-CAMERA - DNN
       # 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"

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_;

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

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,35 +78,50 @@ 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;
+      }
     }
   }
 
   // Update time
   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