Merge pull request #1236 from technolojin/tmp/mot-timer-cherrypicks

fix(multi_object_tracker): mot timer cherrypicks

perception/multi_object_tracker/include/multi_object_tracker/debugger.hpp

@@ -41,7 +41,10 @@ class TrackerDebugger
     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);
   struct DEBUG_SETTINGS
@@ -56,12 +59,15 @@ class TrackerDebugger
 
 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_;
   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_;
 };
 

perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp

@@ -58,7 +58,11 @@ class MultiObjectTracker : public rclcpp::Node
     tracked_objects_pub_;
   rclcpp::Subscription<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
     detected_object_sub_;
-  rclcpp::TimerBase::SharedPtr publish_timer_;  // publish timer
+
+  // publish timer
+  rclcpp::TimerBase::SharedPtr publish_timer_;
+  rclcpp::Time last_published_time_;
+  double publisher_period_;
 
   // debugger class
   std::unique_ptr<TrackerDebugger> debugger_;
@@ -79,14 +83,14 @@ class MultiObjectTracker : public rclcpp::Node
   std::unique_ptr<DataAssociation> data_association_;
 
   void checkTrackerLifeCycle(
-    std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time,
-    const geometry_msgs::msg::Transform & self_transform);
+    std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time);
   void sanitizeTracker(
     std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time);
   std::shared_ptr<Tracker> createNewTracker(
     const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time,
     const geometry_msgs::msg::Transform & self_transform) const;
 
+  void checkAndPublish(const rclcpp::Time & time);
   void publish(const rclcpp::Time & time) const;
   inline bool shouldTrackerPublish(const std::shared_ptr<const Tracker> tracker) const;
 };

perception/multi_object_tracker/src/debugger.cpp

@@ -18,12 +18,7 @@
 
 #include <memory>
 
-TrackerDebugger::TrackerDebugger(rclcpp::Node & node)
-: diagnostic_updater_(&node),
-  node_(node),
-  last_input_stamp_(node.now()),
-  stamp_process_start_(node.now()),
-  stamp_publish_output_(node.now())
+TrackerDebugger::TrackerDebugger(rclcpp::Node & node) : diagnostic_updater_(&node), node_(node)
 {
   // declare debug parameters to decide whether to publish debug topics
   loadParameters();
@@ -39,7 +34,15 @@ TrackerDebugger::TrackerDebugger(rclcpp::Node & node)
         "debug/tentative_objects", rclcpp::QoS{1});
   }
 
-  // initialize stop watch and diagnostics
+  // initialize timestamps
+  const rclcpp::Time now = node_.now();
+  last_input_stamp_ = now;
+  stamp_process_start_ = now;
+  stamp_process_end_ = now;
+  stamp_publish_start_ = now;
+  stamp_publish_output_ = now;
+
+  // setup diagnostics
   setupDiagnostics();
 }
 
@@ -73,7 +76,11 @@ void TrackerDebugger::loadParameters()
 
 void TrackerDebugger::checkDelay(diagnostic_updater::DiagnosticStatusWrapper & stat)
 {
-  const double delay = pipeline_latency_ms_;  // [s]
+  if (!is_initialized_) {
+    stat.summary(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Measurement time is not set.");
+    return;
+  }
+  const double & delay = pipeline_latency_ms_;  // [s]
 
   if (delay == 0.0) {
     stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Detection delay is not calculated.");
@@ -108,18 +115,40 @@ void TrackerDebugger::startMeasurementTime(
     processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
       "debug/input_latency_ms", input_latency_ms);
   }
+  // initialize debug time stamps
+  if (!is_initialized_) {
+    stamp_publish_output_ = now;
+    is_initialized_ = true;
+  }
+}
+
+void TrackerDebugger::endMeasurementTime(const rclcpp::Time & now)
+{
+  stamp_process_end_ = now;
+}
+
+void TrackerDebugger::startPublishTime(const rclcpp::Time & now)
+{
+  stamp_publish_start_ = now;
 }
 
 void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Time & object_time)
 {
-  const auto current_time = now;
-  // pipeline latency: time from sensor measurement to publish
-  pipeline_latency_ms_ = (current_time - last_input_stamp_).seconds() * 1e3;
+  // if the measurement time is not set, do not publish debug information
+  if (!is_initialized_) return;
+
+  // pipeline latency: time from sensor measurement to publish, used for 'checkDelay'
+  pipeline_latency_ms_ = (now - last_input_stamp_).seconds() * 1e3;
+
   if (debug_settings_.publish_processing_time) {
-    // processing time: time between input and publish
-    double processing_time_ms = (current_time - stamp_process_start_).seconds() * 1e3;
+    // processing latency: time between input and publish
+    double processing_latency_ms = ((now - stamp_process_start_).seconds()) * 1e3;
+    // processing time: only the time spent in the tracking processes
+    double processing_time_ms = ((now - stamp_publish_start_).seconds() +
+                                 (stamp_process_end_ - stamp_process_start_).seconds()) *
+                                1e3;
     // cycle time: time between two consecutive publish
-    double cyclic_time_ms = (current_time - stamp_publish_output_).seconds() * 1e3;
+    double cyclic_time_ms = (now - stamp_publish_output_).seconds() * 1e3;
     // measurement to tracked-object time: time from the sensor measurement to the publishing object
     // time If there is not latency compensation, this value is zero
     double measurement_to_object_ms = (object_time - last_input_stamp_).seconds() * 1e3;
@@ -131,8 +160,10 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim
       "debug/cyclic_time_ms", cyclic_time_ms);
     processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
       "debug/processing_time_ms", processing_time_ms);
+    processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
+      "debug/processing_latency_ms", processing_latency_ms);
     processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
       "debug/meas_to_tracked_object_ms", measurement_to_object_ms);
   }
-  stamp_publish_output_ = current_time;
+  stamp_publish_output_ = now;
 }

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -68,6 +68,7 @@ boost::optional<geometry_msgs::msg::Transform> getTransformAnonymous(
 
 MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
 : rclcpp::Node("multi_object_tracker", node_options),
+  last_published_time_(this->now()),
   tf_buffer_(this->get_clock()),
   tf_listener_(tf_buffer_)
 {
@@ -83,7 +84,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     create_publisher<autoware_auto_perception_msgs::msg::TrackedObjects>("output", rclcpp::QoS{1});
 
   // Parameters
-  double publish_rate = declare_parameter<double>("publish_rate");
+  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")};
 
@@ -94,11 +95,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
     this->get_node_base_interface(), this->get_node_timers_interface());
   tf_buffer_.setCreateTimerInterface(cti);
 
-  // Create ROS time based timer
+  // Create ROS time based timer.
+  // If the delay compensation is enabled, the timer is used to publish the output at the correct
+  // time.
   if (enable_delay_compensation) {
-    const auto period_ns = rclcpp::Rate(publish_rate).period();
+    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(), period_ns, std::bind(&MultiObjectTracker::onTimer, this));
+      this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this));
   }
 
   const auto tmp = this->declare_parameter<std::vector<int64_t>>("can_assign_matrix");
@@ -135,10 +140,14 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
 void MultiObjectTracker::onMeasurement(
   const autoware_auto_perception_msgs::msg::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());
+
   /* keep the latest input stamp and check transform*/
-  debugger_->startMeasurementTime(this->now(), rclcpp::Time(input_objects_msg->header.stamp));
-  const auto self_transform = getTransformAnonymous(
-    tf_buffer_, "base_link", world_frame_id_, input_objects_msg->header.stamp);
+  debugger_->startMeasurementTime(this->now(), measurement_time);
+  const auto self_transform =
+    getTransformAnonymous(tf_buffer_, "base_link", world_frame_id_, measurement_time);
   if (!self_transform) {
     return;
   }
@@ -150,7 +159,6 @@ void MultiObjectTracker::onMeasurement(
     return;
   }
   /* tracker prediction */
-  const rclcpp::Time measurement_time = input_objects_msg->header.stamp;
   for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) {
     (*itr)->predict(measurement_time);
   }
@@ -176,7 +184,7 @@ void MultiObjectTracker::onMeasurement(
   }
 
   /* life cycle check */
-  checkTrackerLifeCycle(list_tracker_, measurement_time, *self_transform);
+  checkTrackerLifeCycle(list_tracker_, measurement_time);
   /* sanitize trackers */
   sanitizeTracker(list_tracker_, measurement_time);
 
@@ -190,8 +198,19 @@ void MultiObjectTracker::onMeasurement(
     if (tracker) list_tracker_.push_back(tracker);
   }
 
+  // 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());
+    }
   }
 }
 
@@ -225,24 +244,32 @@ std::shared_ptr<Tracker> MultiObjectTracker::createNewTracker(
 void MultiObjectTracker::onTimer()
 {
   const rclcpp::Time current_time = this->now();
-  const auto self_transform =
-    getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time);
-  if (!self_transform) {
-    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) {
+    checkAndPublish(current_time);
   }
+}
 
+void MultiObjectTracker::checkAndPublish(const rclcpp::Time & time)
+{
   /* life cycle check */
-  checkTrackerLifeCycle(list_tracker_, current_time, *self_transform);
+  checkTrackerLifeCycle(list_tracker_, time);
   /* sanitize trackers */
-  sanitizeTracker(list_tracker_, current_time);
+  sanitizeTracker(list_tracker_, time);
 
   // Publish
-  publish(current_time);
+  publish(time);
+
+  // Update last published time
+  last_published_time_ = this->now();
 }
 
 void MultiObjectTracker::checkTrackerLifeCycle(
-  std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time,
-  [[maybe_unused]] const geometry_msgs::msg::Transform & self_transform)
+  std::list<std::shared_ptr<Tracker>> & list_tracker, const rclcpp::Time & time)
 {
   /* params */
   constexpr float max_elapsed_time = 1.0;
@@ -337,6 +364,7 @@ inline bool MultiObjectTracker::shouldTrackerPublish(
 
 void MultiObjectTracker::publish(const rclcpp::Time & time) const
 {
+  debugger_->startPublishTime(this->now());
   const auto subscriber_count = tracked_objects_pub_->get_subscription_count() +
                                 tracked_objects_pub_->get_intra_process_subscription_count();
   if (subscriber_count < 1) {