Skip to content

Commit 5f3f519

Browse files
technolojinKhalilSelyan
authored and
KhalilSelyan
committed
fix(multi_object_tracker): input stream timimg functions (#7127)
* fix: rename to latest_exported_object_time_ Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * fix: add time range checks Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> * chore: add comments, refactoring Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp> --------- Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>
1 parent 845099c commit 5f3f519

File tree

2 files changed

+58
-32
lines changed

2 files changed

+58
-32
lines changed

perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp

+9-7
Original file line numberDiff line numberDiff line change
@@ -117,17 +117,13 @@ class InputManager
117117
{
118118
public:
119119
explicit InputManager(rclcpp::Node & node);
120-
121120
void init(const std::vector<InputChannel> & input_channels);
122-
void setTriggerFunction(std::function<void()> func_trigger) { func_trigger_ = func_trigger; }
123121

122+
void setTriggerFunction(std::function<void()> func_trigger) { func_trigger_ = func_trigger; }
124123
void onTrigger(const uint & index) const;
125124

126-
void getObjectTimeInterval(
127-
const rclcpp::Time & now, rclcpp::Time & object_latest_time,
128-
rclcpp::Time & object_oldest_time) const;
129-
void optimizeTimings();
130125
bool getObjects(const rclcpp::Time & now, ObjectsList & objects_list);
126+
131127
bool isChannelSpawnEnabled(const uint & index) const
132128
{
133129
return input_streams_[index]->isSpawnEnabled();
@@ -138,7 +134,7 @@ class InputManager
138134
std::vector<rclcpp::Subscription<DetectedObjects>::SharedPtr> sub_objects_array_{};
139135

140136
bool is_initialized_{false};
141-
rclcpp::Time latest_object_time_;
137+
rclcpp::Time latest_exported_object_time_;
142138

143139
size_t input_size_;
144140
std::vector<std::shared_ptr<InputStream>> input_streams_;
@@ -151,6 +147,12 @@ class InputManager
151147
double target_stream_interval_std_{0.02}; // [s]
152148
double target_latency_{0.2}; // [s]
153149
double target_latency_band_{1.0}; // [s]
150+
151+
private:
152+
void getObjectTimeInterval(
153+
const rclcpp::Time & now, rclcpp::Time & object_latest_time,
154+
rclcpp::Time & object_oldest_time) const;
155+
void optimizeTimings();
154156
};
155157

156158
} // namespace multi_object_tracker

perception/multi_object_tracker/src/processor/input_manager.cpp

+49-25
Original file line numberDiff line numberDiff line change
@@ -127,15 +127,16 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim
127127
// Update time
128128
latest_message_time_ = now;
129129
constexpr double delay_threshold = 3.0; // [s]
130-
if (std::abs((latest_measurement_time_ - objects_time).seconds()) > delay_threshold) {
131-
// Reset the latest measurement time if the time difference is too large
130+
if (objects_time < latest_measurement_time_ - rclcpp::Duration::from_seconds(delay_threshold)) {
131+
// If the given object time is older than the latest measurement time by more than the
132+
// threshold, the system time may have been reset. Reset the latest measurement time
132133
latest_measurement_time_ = objects_time;
133134
RCLCPP_WARN(
134135
node_.get_logger(),
135136
"InputManager::updateTimingStatus %s: Resetting the latest measurement time to %f",
136137
long_name_.c_str(), objects_time.seconds());
137138
} else {
138-
// Aware reversed message arrival
139+
// Update only if the object time is newer than the latest measurement time
139140
latest_measurement_time_ =
140141
latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_;
141142
}
@@ -150,7 +151,14 @@ void InputStream::getObjectsOlderThan(
150151
const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
151152
ObjectsList & objects_list)
152153
{
153-
assert(object_latest_time.nanoseconds() > object_oldest_time.nanoseconds());
154+
if (object_latest_time < object_oldest_time) {
155+
RCLCPP_WARN(
156+
node_.get_logger(),
157+
"InputManager::getObjectsOlderThan %s: Invalid object time interval, object_latest_time: %f, "
158+
"object_oldest_time: %f",
159+
long_name_.c_str(), object_latest_time.seconds(), object_oldest_time.seconds());
160+
return;
161+
}
154162

155163
for (const auto & object : objects_que_) {
156164
const rclcpp::Time object_time = rclcpp::Time(object.header.stamp);
@@ -176,7 +184,7 @@ void InputStream::getObjectsOlderThan(
176184
////////////////////////////
177185
InputManager::InputManager(rclcpp::Node & node) : node_(node)
178186
{
179-
latest_object_time_ = node_.now();
187+
latest_exported_object_time_ = node_.now() - rclcpp::Duration::from_seconds(3.0);
180188
}
181189

182190
void InputManager::init(const std::vector<InputChannel> & input_channels)
@@ -230,26 +238,37 @@ void InputManager::getObjectTimeInterval(
230238
const rclcpp::Time & now, rclcpp::Time & object_latest_time,
231239
rclcpp::Time & object_oldest_time) const
232240
{
241+
// Set the object time interval
242+
243+
// 1. object_latest_time
244+
// The object_latest_time is the current time minus the target stream latency
233245
object_latest_time =
234-
now - rclcpp::Duration::from_seconds(
235-
target_stream_latency_ -
236-
0.1 * target_stream_latency_std_); // object_latest_time with 0.1 sigma safety margin
246+
now - rclcpp::Duration::from_seconds(target_stream_latency_ - 0.1 * target_stream_latency_std_);
247+
237248
// check the target stream can be included in the object time interval
238249
if (input_streams_.at(target_stream_idx_)->isTimeInitialized()) {
239-
rclcpp::Time latest_measurement_time =
250+
const rclcpp::Time latest_measurement_time =
240251
input_streams_.at(target_stream_idx_)->getLatestMeasurementTime();
241-
242-
// if the object_latest_time is older than the latest measurement time, set it as the latest
252+
// if the object_latest_time is older than the latest measurement time, set it to the latest
243253
// object time
244254
object_latest_time =
245255
object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time;
246256
}
247257

248-
object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds(1.0);
249-
// if the object_oldest_time is older than the latest object time, set it to the latest object
250-
// time
251-
object_oldest_time =
252-
object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_;
258+
// 2. object_oldest_time
259+
// The default object_oldest_time is to have a 1-second time interval
260+
const rclcpp::Time object_oldest_time_default =
261+
object_latest_time - rclcpp::Duration::from_seconds(1.0);
262+
if (latest_exported_object_time_ < object_oldest_time_default) {
263+
// if the latest exported object time is too old, set to the default
264+
object_oldest_time = object_oldest_time_default;
265+
} else if (latest_exported_object_time_ > object_latest_time) {
266+
// if the latest exported object time is newer than the object_latest_time, set to the default
267+
object_oldest_time = object_oldest_time_default;
268+
} else {
269+
// The object_oldest_time is the latest exported object time
270+
object_oldest_time = latest_exported_object_time_;
271+
}
253272
}
254273

255274
void InputManager::optimizeTimings()
@@ -298,7 +317,8 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
298317
objects_list.clear();
299318

300319
// Get the time interval for the objects
301-
rclcpp::Time object_latest_time, object_oldest_time;
320+
rclcpp::Time object_latest_time;
321+
rclcpp::Time object_oldest_time;
302322
getObjectTimeInterval(now, object_latest_time, object_oldest_time);
303323

304324
// Optimize the target stream, latency, and its band
@@ -320,20 +340,24 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
320340
0;
321341
});
322342

323-
// Update the latest object time
343+
// Update the latest exported object time
324344
bool is_any_object = !objects_list.empty();
325345
if (is_any_object) {
326-
latest_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp);
346+
latest_exported_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp);
327347
} else {
328-
// check time jump
329-
if (now < latest_object_time_) {
348+
// check time jump back
349+
if (now < latest_exported_object_time_) {
330350
RCLCPP_WARN(
331351
node_.get_logger(),
332-
"InputManager::getObjects Time jump detected, now: %f, latest_object_time_: %f",
333-
now.seconds(), latest_object_time_.seconds());
334-
latest_object_time_ =
335-
now - rclcpp::Duration::from_seconds(3.0); // reset the latest object time to 3 seconds ago
352+
"InputManager::getObjects Detected jump back in time, now: %f, "
353+
"latest_exported_object_time_: %f",
354+
now.seconds(), latest_exported_object_time_.seconds());
355+
// reset the latest exported object time to 3 seconds ago,
356+
const rclcpp::Time latest_exported_object_time_default =
357+
now - rclcpp::Duration::from_seconds(3.0);
358+
latest_exported_object_time_ = latest_exported_object_time_default;
336359
} else {
360+
// No objects in the object list, no update for the latest exported object time
337361
RCLCPP_DEBUG(
338362
node_.get_logger(),
339363
"InputManager::getObjects No objects in the object list, object time band from %f to %f",

0 commit comments

Comments
 (0)