12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Wolfgang Hess 2016-09-27 16:39:25 +02:00 committed by GitHub
parent 152e68cd1a
commit 0e15fb9a87
1 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
cartographer_ros/src/cartographer_node_main.cc
View File

@ -283,9 +283,20 @@ Node::~Node() {
Rigid3d Node::LookupToTrackingTransformOrThrow(const carto::common::Time time,
const string& frame_id) {
return ToRigid3d(tf_buffer_.lookupTransform(
options_.tracking_frame, frame_id, ToRos(time),
::ros::Duration(options_.lookup_transform_timeout_sec)));
::ros::Duration timeout(options_.lookup_transform_timeout_sec);
const ::ros::Time latest_tf_time =
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,