@@ -127,15 +127,16 @@ void InputStream::updateTimingStatus(const rclcpp::Time & now, const rclcpp::Tim
127
127
// Update time
128
128
latest_message_time_ = now;
129
129
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
132
133
latest_measurement_time_ = objects_time;
133
134
RCLCPP_WARN (
134
135
node_.get_logger (),
135
136
" InputManager::updateTimingStatus %s: Resetting the latest measurement time to %f" ,
136
137
long_name_.c_str (), objects_time.seconds ());
137
138
} else {
138
- // Aware reversed message arrival
139
+ // Update only if the object time is newer than the latest measurement time
139
140
latest_measurement_time_ =
140
141
latest_measurement_time_ < objects_time ? objects_time : latest_measurement_time_;
141
142
}
@@ -237,36 +238,36 @@ void InputManager::getObjectTimeInterval(
237
238
const rclcpp::Time & now, rclcpp::Time & object_latest_time,
238
239
rclcpp::Time & object_oldest_time) const
239
240
{
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
240
245
object_latest_time =
241
- now - rclcpp::Duration::from_seconds (
242
- target_stream_latency_ -
243
- 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
+
244
248
// check the target stream can be included in the object time interval
245
249
if (input_streams_.at (target_stream_idx_)->isTimeInitialized ()) {
246
- rclcpp::Time latest_measurement_time =
250
+ const rclcpp::Time latest_measurement_time =
247
251
input_streams_.at (target_stream_idx_)->getLatestMeasurementTime ();
248
-
249
252
// if the object_latest_time is older than the latest measurement time, set it to the latest
250
253
// object time
251
254
object_latest_time =
252
255
object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time;
253
256
}
254
257
255
- object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds (1.0 );
256
- // if the object_oldest_time is older than the latest object time, set it to the latest object
257
- // time
258
- object_oldest_time = object_oldest_time < latest_exported_object_time_
259
- ? latest_exported_object_time_
260
- : object_oldest_time;
261
-
262
- // check the object time interval is valid
263
- if (object_oldest_time > object_latest_time) {
264
- RCLCPP_WARN (
265
- node_.get_logger (),
266
- " InputManager::getObjectTimeInterval Invalid object time interval, object_latest_time: %f, "
267
- " object_oldest_time: %f" ,
268
- (now - object_latest_time).seconds (), (now - object_oldest_time).seconds ());
269
- object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds (1.0 );
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_;
270
271
}
271
272
}
272
273
@@ -316,7 +317,8 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
316
317
objects_list.clear ();
317
318
318
319
// Get the time interval for the objects
319
- rclcpp::Time object_latest_time, object_oldest_time;
320
+ rclcpp::Time object_latest_time;
321
+ rclcpp::Time object_oldest_time;
320
322
getObjectTimeInterval (now, object_latest_time, object_oldest_time);
321
323
322
324
// Optimize the target stream, latency, and its band
@@ -343,16 +345,19 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
343
345
if (is_any_object) {
344
346
latest_exported_object_time_ = rclcpp::Time (objects_list.back ().second .header .stamp );
345
347
} else {
346
- // check time jump
348
+ // check time jump back
347
349
if (now < latest_exported_object_time_) {
348
350
RCLCPP_WARN (
349
351
node_.get_logger (),
350
- " InputManager::getObjects Time jump detected, now: %f, latest_exported_object_time_: %f" ,
352
+ " InputManager::getObjects Detected jump back in time, now: %f, "
353
+ " latest_exported_object_time_: %f" ,
351
354
now.seconds (), latest_exported_object_time_.seconds ());
352
- latest_exported_object_time_ =
353
- now - rclcpp::Duration::from_seconds (
354
- 3.0 ); // reset the latest exported object time to 3 seconds ago
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;
355
359
} else {
360
+ // No objects in the object list, no update for the latest exported object time
356
361
RCLCPP_DEBUG (
357
362
node_.get_logger (),
358
363
" InputManager::getObjects No objects in the object list, object time band from %f to %f" ,
0 commit comments