Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(multi_object_tracker): input stream timimg functions #7127

Merged
merged 3 commits into from
May 29, 2024
Merged

fix(multi_object_tracker): input stream timimg functions #7127

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
07536f2
fix: rename to latest_exported_object_time_
technolojin May 27, 2024
39ac546
fix: add time range checks
technolojin May 27, 2024
94bdca4
chore: add comments, refactoring
technolojin May 27, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 9 additions & 7 deletions perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp
View file
Open in desktop
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
Open in desktop
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
Loading