From b623e444e7a62af93aeea2c28aecae5bc546a5aa Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 31 Jul 2024 14:20:34 +0900 Subject: [PATCH 1/3] fix: revert latency reduction logic and bring back to timer trigger Signed-off-by: Taekjin LEE --- .../src/multi_object_tracker_core.cpp | 24 +------------------ 1 file changed, 1 insertion(+), 23 deletions(-) diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 173cec9489f63..e59b4bffb00f4 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -154,7 +154,7 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // time. if (enable_delay_compensation) { publisher_period_ = 1.0 / publish_rate; // [s] - constexpr double timer_multiplier = 20.0; // 20 times frequent for publish timing check + constexpr double timer_multiplier = 1.0; // 2 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)); @@ -210,35 +210,13 @@ void MultiObjectTracker::onTrigger() std::vector> objects_list; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); if (!is_objects_ready) return; - onMessage(objects_list); - const rclcpp::Time latest_time(objects_list.back().second.header.stamp); - - // Publish objects if the timer is not enabled - if (publish_timer_ == nullptr) { - // if the delay compensation is disabled, publish the objects in the latest time - publish(latest_time); - } else { - // Publish if the next publish time is close - const double minimum_publish_interval = publisher_period_ * 0.70; // 70% of the period - const rclcpp::Time publish_time = this->now(); - if ((publish_time - last_published_time_).seconds() > minimum_publish_interval) { - checkAndPublish(publish_time); - } - } } void MultiObjectTracker::onTimer() { const rclcpp::Time current_time = this->now(); - // 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) return; - // get objects from the input manager and run process ObjectsList objects_list; const bool is_objects_ready = input_manager_->getObjects(current_time, objects_list); From 6cee80e57807336a2836f221f69dcfba4170614d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 31 Jul 2024 05:26:10 +0000 Subject: [PATCH 2/3] style(pre-commit): autofix --- .../multi_object_tracker/src/multi_object_tracker_core.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index e59b4bffb00f4..c7df6d9eacab8 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -153,7 +153,7 @@ 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) { - publisher_period_ = 1.0 / publish_rate; // [s] + publisher_period_ = 1.0 / publish_rate; // [s] constexpr double timer_multiplier = 1.0; // 2 times frequent for publish timing check const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period(); publish_timer_ = rclcpp::create_timer( From 59d6c3e5e980b1b6d92d5e6b59c74b90d9c72fe9 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Wed, 31 Jul 2024 15:15:08 +0900 Subject: [PATCH 3/3] chore: remove unused variables Signed-off-by: Taekjin LEE --- .../multi_object_tracker/multi_object_tracker_core.hpp | 1 - .../multi_object_tracker/src/multi_object_tracker_core.cpp | 4 +--- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp index 4c1ccced1bcf7..d6b79e13f1d3d 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp @@ -78,7 +78,6 @@ class MultiObjectTracker : public rclcpp::Node // publish timer rclcpp::TimerBase::SharedPtr publish_timer_; rclcpp::Time last_published_time_; - double publisher_period_; // internal states std::string world_frame_id_; // tracking frame diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index c7df6d9eacab8..94062bcf9cc81 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -153,9 +153,7 @@ 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) { - publisher_period_ = 1.0 / publish_rate; // [s] - constexpr double timer_multiplier = 1.0; // 2 times frequent for publish timing check - const auto timer_period = rclcpp::Rate(publish_rate * timer_multiplier).period(); + const auto timer_period = rclcpp::Rate(publish_rate).period(); publish_timer_ = rclcpp::create_timer( this, get_clock(), timer_period, std::bind(&MultiObjectTracker::onTimer, this)); }