From b6e9a6bb7efcd047a5cfd74ea5111a714162b769 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 27 May 2024 13:16:20 +0900 Subject: [PATCH] chore: add comments, refactoring Signed-off-by: Taekjin LEE --- .../processor/input_manager.hpp | 14 +++-- .../src/processor/input_manager.cpp | 63 ++++++++++--------- 2 files changed, 42 insertions(+), 35 deletions(-) diff --git a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp index 6782da0c72dee..f46c088c6d508 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp @@ -117,17 +117,13 @@ class InputManager { public: explicit InputManager(rclcpp::Node & node); - void init(const std::vector & input_channels); - void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } + void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } void onTrigger(const uint & index) const; - void getObjectTimeInterval( - const rclcpp::Time & now, rclcpp::Time & object_latest_time, - rclcpp::Time & object_oldest_time) const; - void optimizeTimings(); bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list); + bool isChannelSpawnEnabled(const uint & index) const { return input_streams_[index]->isSpawnEnabled(); @@ -151,6 +147,12 @@ class InputManager double target_stream_interval_std_{0.02}; // [s] double target_latency_{0.2}; // [s] double target_latency_band_{1.0}; // [s] + +private: + void getObjectTimeInterval( + const rclcpp::Time & now, rclcpp::Time & object_latest_time, + rclcpp::Time & object_oldest_time) const; + void optimizeTimings(); }; } // namespace multi_object_tracker diff --git a/perception/multi_object_tracker/src/processor/input_manager.cpp b/perception/multi_object_tracker/src/processor/input_manager.cpp index 2be8b21717e8f..44bdf09046ccd 100644 --- a/perception/multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/multi_object_tracker/src/processor/input_manager.cpp @@ -127,15 +127,16 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim // Update time latest_message_time_ = now; constexpr double delay_threshold = 3.0; // [s] - if (std::abs((latest_measurement_time_ - objects_time).seconds()) > delay_threshold) { - // Reset the latest measurement time if the time difference is too large + if (objects_time < latest_measurement_time_ - rclcpp::Duration::from_seconds(delay_threshold)) { + // If the given object time is older than the latest measurement time by more than the + // threshold, the system time may have been reset. Reset the latest measurement time latest_measurement_time_ = objects_time; RCLCPP_WARN( node_.get_logger(), "InputManager::updateTimingStatus %s: Resetting the latest measurement time to %f", long_name_.c_str(), objects_time.seconds()); } else { - // Aware reversed message arrival + // Update only if the object time is newer than the latest measurement time latest_measurement_time_ = latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_; } @@ -237,36 +238,36 @@ void InputManager::getObjectTimeInterval( const rclcpp::Time & now, rclcpp::Time & object_latest_time, rclcpp::Time & object_oldest_time) const { + // Set the object time interval + + // 1. object_latest_time + // The object_latest_time is the current time minus the target stream latency object_latest_time = - now - rclcpp::Duration::from_seconds( - target_stream_latency_ - - 0.1 * target_stream_latency_std_); // object_latest_time with 0.1 sigma safety margin + now - rclcpp::Duration::from_seconds(target_stream_latency_ - 0.1 * target_stream_latency_std_); + // check the target stream can be included in the object time interval if (input_streams_.at(target_stream_idx_)->isTimeInitialized()) { - rclcpp::Time latest_measurement_time = + const rclcpp::Time latest_measurement_time = input_streams_.at(target_stream_idx_)->getLatestMeasurementTime(); - // if the object_latest_time is older than the latest measurement time, set it to the latest // object time object_latest_time = object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time; } - object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0); - // if the object_oldest_time is older than the latest object time, set it to the latest object - // time - object_oldest_time = object_oldest_time < latest_exported_object_time_ - ? latest_exported_object_time_ - : object_oldest_time; - - // check the object time interval is valid - if (object_oldest_time > object_latest_time) { - RCLCPP_WARN( - node_.get_logger(), - "InputManager::getObjectTimeInterval Invalid object time interval, object_latest_time: %f, " - "object_oldest_time: %f", - (now - object_latest_time).seconds(), (now - object_oldest_time).seconds()); - object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0); + // 2. object_oldest_time + // The default object_oldest_time is to have a 1-second time interval + const rclcpp::Time object_oldest_time_default = + object_latest_time - rclcpp::Duration::from_seconds(1.0); + if (latest_exported_object_time_ < object_oldest_time_default) { + // if the latest exported object time is too old, set to the default + object_oldest_time = object_oldest_time_default; + } else if (latest_exported_object_time_ > object_latest_time) { + // if the latest exported object time is newer than the object_latest_time, set to the default + object_oldest_time = object_oldest_time_default; + } else { + // The object_oldest_time is the latest exported object time + object_oldest_time = latest_exported_object_time_; } } @@ -316,7 +317,8 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li objects_list.clear(); // Get the time interval for the objects - rclcpp::Time object_latest_time, object_oldest_time; + rclcpp::Time object_latest_time; + rclcpp::Time object_oldest_time; getObjectTimeInterval(now, object_latest_time, object_oldest_time); // Optimize the target stream, latency, and its band @@ -343,16 +345,19 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li if (is_any_object) { latest_exported_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); } else { - // check time jump + // check time jump back if (now < latest_exported_object_time_) { RCLCPP_WARN( node_.get_logger(), - "InputManager::getObjects Time jump detected, now: %f, latest_exported_object_time_: %f", + "InputManager::getObjects Detected jump back in time, now: %f, " + "latest_exported_object_time_: %f", now.seconds(), latest_exported_object_time_.seconds()); - latest_exported_object_time_ = - now - rclcpp::Duration::from_seconds( - 3.0); // reset the latest exported object time to 3 seconds ago + // reset the latest exported object time to 3 seconds ago, + const rclcpp::Time latest_exported_object_time_default = + now - rclcpp::Duration::from_seconds(3.0); + latest_exported_object_time_ = latest_exported_object_time_default; } else { + // No objects in the object list, no update for the latest exported object time RCLCPP_DEBUG( node_.get_logger(), "InputManager::getObjects No objects in the object list, object time band from %f to %f",