From 544b06eea2d6f9517f3ae8005146f557d3994a70 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 2 Sep 2024 09:40:27 +0900 Subject: [PATCH] feat(autoware_multi_object_tracker): reduce trigger latency (#8657) * feat: timer-based trigger with phase compensation Signed-off-by: Taekjin LEE * chore: update comments, name of variable Signed-off-by: Taekjin LEE * chore: declare min and max publish interval ratios Signed-off-by: Taekjin LEE * style(pre-commit): autofix --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/multi_object_tracker_node.cpp | 30 +++++++++++++------ .../src/multi_object_tracker_node.hpp | 4 +++ 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index 4b3240d81e68f..ce7f8157a31b6 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -73,7 +73,8 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("multi_object_tracker", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_), - last_published_time_(this->now()) + last_published_time_(this->now()), + last_updated_time_(this->now()) { // glog for debug if (!google::IsGoogleLoggingInitialized()) { @@ -153,7 +154,9 @@ 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) { - const auto timer_period = rclcpp::Rate(publish_rate).period(); + publisher_period_ = 1.0 / publish_rate; // [s] + constexpr double timer_multiplier = 10.0; // 10 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)); } @@ -221,21 +224,30 @@ void MultiObjectTracker::onTimer() { const rclcpp::Time current_time = this->now(); - // get objects from the input manager and run process - ObjectsList objects_list; - const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); - if (is_objects_ready) { - onMessage(objects_list); + // ensure minimum interval: room for the next process(prediction) + const double minimum_publish_interval = publisher_period_ * minimum_publish_interval_ratio; + const auto elapsed_time = (current_time - last_published_time_).seconds(); + if (elapsed_time < minimum_publish_interval) { + return; } - // Publish with delay compensation - checkAndPublish(current_time); + // if there was update after publishing, publish new messages + bool should_publish = last_published_time_ < last_updated_time_; + + // if there was no update, publish if the elapsed time is longer than the maximum publish latency + // in this case, it will perform extrapolate/remove old objects + const double maximum_publish_interval = publisher_period_ * maximum_publish_interval_ratio; + should_publish = should_publish || elapsed_time > maximum_publish_interval; + + // Publish with delay compensation to the current time + if (should_publish) checkAndPublish(current_time); } 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); + last_updated_time_ = current_time; // process start debugger_->startMeasurementTime(this->now(), oldest_time); diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index e917acbda9fcc..3c23e3f066488 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -78,6 +78,10 @@ class MultiObjectTracker : public rclcpp::Node // publish timer rclcpp::TimerBase::SharedPtr publish_timer_; rclcpp::Time last_published_time_; + rclcpp::Time last_updated_time_; + double publisher_period_; + static constexpr double minimum_publish_interval_ratio = 0.85; + static constexpr double maximum_publish_interval_ratio = 1.05; // internal states std::string world_frame_id_; // tracking frame