Quickly skip over data at the beginning. (#70)
Sensor data that precedes the published tf transforms can likely never be transformed, so it should be skipped without delay.master
parent
152e68cd1a
commit
0e15fb9a87
|
@ -283,9 +283,20 @@ Node::~Node() {
|
||||||
|
|
||||||
Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time,
|
Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time,
|
||||||
const string& frame_id) {
|
const string& frame_id) {
|
||||||
return ToRigid3d(tf_buffer_.lookupTransform(
|
::ros::Duration timeout(options_.lookup_transform_timeout_sec);
|
||||||
options_.tracking_frame, frame_id, ToRos(time),
|
const ::ros::Time latest_tf_time =
|
||||||
::ros::Duration(options_.lookup_transform_timeout_sec)));
|
tf_buffer_
|
||||||
|
.lookupTransform(options_.tracking_frame, frame_id, ::ros::Time(0.),
|
||||||
|
timeout)
|
||||||
|
.header.stamp;
|
||||||
|
const ::ros::Time requested_time = ToRos(time);
|
||||||
|
if (latest_tf_time >= requested_time) {
|
||||||
|
// We already have newer data, so we do not wait. Otherwise, we would wait
|
||||||
|
// for the full 'timeout' even if we ask for data that is too old.
|
||||||
|
timeout = ::ros::Duration(0.);
|
||||||
|
}
|
||||||
|
return ToRigid3d(tf_buffer_.lookupTransform(options_.tracking_frame, frame_id,
|
||||||
|
requested_time, timeout));
|
||||||
}
|
}
|
||||||
|
|
||||||
void Node::AddOdometry(const int64 timestamp, const string& frame_id,
|
void Node::AddOdometry(const int64 timestamp, const string& frame_id,
|
||||||
|
|
Loading…
Reference in New Issue