Skip to content

Commit

Permalink
fix(multi_object_tracker): input stream timimg functions (#7127)
Browse files Browse the repository at this point in the history 
* fix: rename to latest_exported_object_time_

Signed-off-by: Taekjin LEE <[email protected]>

* fix: add time range checks

Signed-off-by: Taekjin LEE <[email protected]>

* chore: add comments, refactoring

Signed-off-by: Taekjin LEE <[email protected]>

---------

Signed-off-by: Taekjin LEE <[email protected]>
  • Loading branch information
@technolojin
technolojin authored May 29, 2024
1 parent 57d6df8 commit 8237b7d
Show file tree
Hide file tree
Showing 2 changed files with 58 additions and 32 deletions.
16 changes: 9 additions & 7 deletions perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,17 +117,13 @@ class InputManager
{
public:
explicit InputManager(rclcpp::Node & node);

void init(const std::vector<InputChannel> & input_channels);
void setTriggerFunction(std::function<void()> func_trigger) { func_trigger_ = func_trigger; }

void setTriggerFunction(std::function<void()> 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();
Expand All @@ -138,7 +134,7 @@ class InputManager
std::vector<rclcpp::Subscription<DetectedObjects>::SharedPtr> sub_objects_array_{};

bool is_initialized_{false};
rclcpp::Time latest_object_time_;
rclcpp::Time latest_exported_object_time_;

size_t input_size_;
std::vector<std::shared_ptr<InputStream>> input_streams_;
Expand All @@ -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
Expand Down
74 changes: 49 additions & 25 deletions perception/multi_object_tracker/src/processor/input_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
}
Expand All @@ -150,7 +151,14 @@ void InputStream::getObjectsOlderThan(
const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
ObjectsList & objects_list)
{
assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds());
if (object_latest_time < object_oldest_time) {
RCLCPP_WARN(
node_.get_logger(),
"InputManager::getObjectsOlderThan %s: Invalid object time interval, object_latest_time: %f, "
"object_oldest_time: %f",
long_name_.c_str(), object_latest_time.seconds(), object_oldest_time.seconds());
return;
}

for (const auto & object : objects_que_) {
const rclcpp::Time object_time = rclcpp::Time(object.header.stamp);
Expand All @@ -176,7 +184,7 @@ void InputStream::getObjectsOlderThan(
////////////////////////////
InputManager::InputManager(rclcpp::Node & node) : node_(node)
{
latest_object_time_ = node_.now();
latest_exported_object_time_ = node_.now() - rclcpp::Duration::from_seconds(3.0);
}

void InputManager::init(const std::vector<InputChannel> & input_channels)
Expand Down Expand Up @@ -230,26 +238,37 @@ 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 as the latest
// 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_object_time_ ? object_oldest_time : latest_object_time_;
// 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_;
}
}

void InputManager::optimizeTimings()
Expand Down Expand Up @@ -298,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
Expand All @@ -320,20 +340,24 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
0;
});

// Update the latest object time
// Update the latest exported object time
bool is_any_object = !objects_list.empty();
if (is_any_object) {
latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp);
latest_exported_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp);
} else {
// check time jump
if (now < latest_object_time_) {
// check time jump back
if (now < latest_exported_object_time_) {
RCLCPP_WARN(
node_.get_logger(),
"InputManager::getObjects Time jump detected, now: %f, latest_object_time_: %f",
now.seconds(), latest_object_time_.seconds());
latest_object_time_ =
now - rclcpp::Duration::from_seconds(3.0); // reset the latest object time to 3 seconds ago
"InputManager::getObjects Detected jump back in time, now: %f, "
"latest_exported_object_time_: %f",
now.seconds(), latest_exported_object_time_.seconds());
// 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",
Expand Down

0 comments on commit 8237b7d

Please sign in to comment.