@@ -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
}
@@ -150,7 +151,14 @@ void InputStream::getObjectsOlderThan(
150
151
const rclcpp::Time & object_latest_time, const rclcpp::Time & object_oldest_time,
151
152
ObjectsList & objects_list)
152
153
{
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
+ }
154
162
155
163
for (const auto & object : objects_que_) {
156
164
const rclcpp::Time object_time = rclcpp::Time (object.header .stamp );
@@ -176,7 +184,7 @@ void InputStream::getObjectsOlderThan(
176
184
// //////////////////////////
177
185
InputManager::InputManager (rclcpp::Node & node) : node_(node)
178
186
{
179
- latest_object_time_ = node_.now ();
187
+ latest_exported_object_time_ = node_.now () - rclcpp::Duration::from_seconds ( 3.0 );
180
188
}
181
189
182
190
void InputManager::init (const std::vector<InputChannel> & input_channels)
@@ -230,26 +238,37 @@ void InputManager::getObjectTimeInterval(
230
238
const rclcpp::Time & now, rclcpp::Time & object_latest_time,
231
239
rclcpp::Time & object_oldest_time) const
232
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
233
245
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
+
237
248
// check the target stream can be included in the object time interval
238
249
if (input_streams_.at (target_stream_idx_)->isTimeInitialized ()) {
239
- rclcpp::Time latest_measurement_time =
250
+ const rclcpp::Time latest_measurement_time =
240
251
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
243
253
// object time
244
254
object_latest_time =
245
255
object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time;
246
256
}
247
257
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
+ }
253
272
}
254
273
255
274
void InputManager::optimizeTimings ()
@@ -298,7 +317,8 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
298
317
objects_list.clear ();
299
318
300
319
// 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;
302
322
getObjectTimeInterval (now, object_latest_time, object_oldest_time);
303
323
304
324
// Optimize the target stream, latency, and its band
@@ -320,20 +340,24 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
320
340
0 ;
321
341
});
322
342
323
- // Update the latest object time
343
+ // Update the latest exported object time
324
344
bool is_any_object = !objects_list.empty ();
325
345
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 );
327
347
} else {
328
- // check time jump
329
- if (now < latest_object_time_ ) {
348
+ // check time jump back
349
+ if (now < latest_exported_object_time_ ) {
330
350
RCLCPP_WARN (
331
351
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;
336
359
} else {
360
+ // No objects in the object list, no update for the latest exported object time
337
361
RCLCPP_DEBUG (
338
362
node_.get_logger (),
339
363
" InputManager::getObjects No objects in the object list, object time band from %f to %f" ,
0 commit comments