From f5c30eb55e52d52412a71369cb44eaab775f8ea4 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 9 Apr 2024 16:15:15 +0900 Subject: [PATCH] feat(multi_object_tracker): reduce publish delay (#6710) * feat: implement a new publish timer Signed-off-by: Taekjin LEE * chore: add comments for new variables Signed-off-by: Taekjin LEE * fix: variable name was wrong Signed-off-by: Taekjin LEE fix: reduce lower limit of publish interval Signed-off-by: Taekjin LEE fix: enlarge publish range upper limit Signed-off-by: Taekjin LEE fix: set parameter tested and agreed Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../multi_object_tracker_core.hpp | 10 ++-- .../src/multi_object_tracker_core.cpp | 48 +++++++++++++------ 2 files changed, 41 insertions(+), 17 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 94cc7185bd518..fe41f97cb5a77 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 @@ -58,7 +58,11 @@ class MultiObjectTracker : public rclcpp::Node tracked_objects_pub_; rclcpp::Subscription::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 debugger_; @@ -79,14 +83,14 @@ class MultiObjectTracker : public rclcpp::Node std::unique_ptr data_association_; void checkTrackerLifeCycle( - std::list> & list_tracker, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform); + std::list> & list_tracker, const rclcpp::Time & time); void sanitizeTracker( std::list> & list_tracker, const rclcpp::Time & time); std::shared_ptr 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 tracker) const; }; 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 8ff69ffaeaa9a..785038fb513c9 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -68,6 +68,7 @@ boost::optional 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("output", rclcpp::QoS{1}); // Parameters - double publish_rate = declare_parameter("publish_rate"); + double publish_rate = declare_parameter("publish_rate"); // [hz] world_frame_id_ = declare_parameter("world_frame_id"); bool enable_delay_compensation{declare_parameter("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>("can_assign_matrix"); @@ -179,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); @@ -198,7 +203,14 @@ void MultiObjectTracker::onMeasurement( // 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()); + } } } @@ -232,24 +244,32 @@ std::shared_ptr 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> & list_tracker, const rclcpp::Time & time, - [[maybe_unused]] const geometry_msgs::msg::Transform & self_transform) + std::list> & list_tracker, const rclcpp::Time & time) { /* params */ constexpr float max_elapsed_time = 1.0;