From 26c656776c10c445b2f09b63b351f932c20c0cc8 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 9 Mar 2016 11:24:10 -0500 Subject: [PATCH] fixed laser_geometry laser to point cloud stamp (including scanning time) --- src/CoreWrapper.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/CoreWrapper.cpp b/src/CoreWrapper.cpp index a9562e76..cb7c3817 100644 --- a/src/CoreWrapper.cpp +++ b/src/CoreWrapper.cpp @@ -887,7 +887,9 @@ void CoreWrapper::commonDepthCallback( if(scanMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull()) + if(getTransform(frameId_, + scanMsg->header.frame_id, + scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)).isNull()) { ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scanMsg->header.stamp.toSec()); return; @@ -998,8 +1000,11 @@ void CoreWrapper::commonStereoCallback( if(scanMsg.get() != 0) { // make sure the frame of the laser is updated too - if(getTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp).isNull()) + if(getTransform(frameId_, + scanMsg->header.frame_id, + scanMsg->header.stamp + ros::Duration().fromSec(scanMsg->ranges.size()*scanMsg->time_increment)).isNull()) { + ROS_ERROR("TF of received laser scan topic at time %fs is not set, aborting rtabmap update.", scanMsg->header.stamp.toSec()); return; }