@@ -176,7 +176,7 @@ void InputStream::getObjectsOlderThan(
176
176
// //////////////////////////
177
177
InputManager::InputManager (rclcpp::Node & node) : node_(node)
178
178
{
179
- latest_object_time_ = node_.now ();
179
+ latest_exported_object_time_ = node_.now () - rclcpp::Duration::from_seconds ( 3.0 );
180
180
}
181
181
182
182
void InputManager::init (const std::vector<InputChannel> & input_channels)
@@ -239,7 +239,7 @@ void InputManager::getObjectTimeInterval(
239
239
rclcpp::Time latest_measurement_time =
240
240
input_streams_.at (target_stream_idx_)->getLatestMeasurementTime ();
241
241
242
- // if the object_latest_time is older than the latest measurement time, set it as the latest
242
+ // if the object_latest_time is older than the latest measurement time, set it to the latest
243
243
// object time
244
244
object_latest_time =
245
245
object_latest_time < latest_measurement_time ? latest_measurement_time : object_latest_time;
@@ -248,8 +248,9 @@ void InputManager::getObjectTimeInterval(
248
248
object_oldest_time = object_latest_time - rclcpp::Duration::from_seconds (1.0 );
249
249
// if the object_oldest_time is older than the latest object time, set it to the latest object
250
250
// time
251
- object_oldest_time =
252
- object_oldest_time > latest_object_time_ ? object_oldest_time : latest_object_time_;
251
+ object_oldest_time = object_oldest_time < latest_exported_object_time_
252
+ ? latest_exported_object_time_
253
+ : object_oldest_time;
253
254
}
254
255
255
256
void InputManager::optimizeTimings ()
@@ -320,19 +321,20 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li
320
321
0 ;
321
322
});
322
323
323
- // Update the latest object time
324
+ // Update the latest exported object time
324
325
bool is_any_object = !objects_list.empty ();
325
326
if (is_any_object) {
326
- latest_object_time_ = rclcpp::Time (objects_list.back ().second .header .stamp );
327
+ latest_exported_object_time_ = rclcpp::Time (objects_list.back ().second .header .stamp );
327
328
} else {
328
329
// check time jump
329
- if (now < latest_object_time_ ) {
330
+ if (now < latest_exported_object_time_ ) {
330
331
RCLCPP_WARN (
331
332
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
333
+ " InputManager::getObjects Time jump detected, now: %f, latest_exported_object_time_: %f" ,
334
+ now.seconds (), latest_exported_object_time_.seconds ());
335
+ latest_exported_object_time_ =
336
+ now - rclcpp::Duration::from_seconds (
337
+ 3.0 ); // reset the latest exported object time to 3 seconds ago
336
338
} else {
337
339
RCLCPP_DEBUG (
338
340
node_.get_logger (),
0 commit comments