i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

ros-pkg: rtabmapviz: fixed "tf2::ExtrapolationException" exception crash when trying to transform laser scan to point cloud

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1656 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-08-19 19:29:14 +00:00
parent a5e12a0633
commit d59b07927f
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GuiWrapper.cpp
+5 -5
View File
@@ -649,10 +649,14 @@ void GuiWrapper::depthScanCallback(
{ {
// TF ready? // TF ready?
Transform localTransform; Transform localTransform;
sensor_msgs::PointCloud2 scanOut;
try try
{ {
//transform laser to point cloud and to frameId_
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
tf::StampedTransform tmp; tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp); tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp); localTransform = transformFromTF(tmp);
} }
@@ -662,10 +666,6 @@ void GuiWrapper::depthScanCallback(
return; return;
} }
//transform in frameId_ frame
sensor_msgs::PointCloud2 scanOut;
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
pcl::PointCloud<pcl::PointXYZ> pclScan; pcl::PointCloud<pcl::PointXYZ> pclScan;
pcl::fromROSMsg(scanOut, pclScan); pcl::fromROSMsg(scanOut, pclScan);
cv::Mat scan = util3d::depth2DFromPointCloud(pclScan); cv::Mat scan = util3d::depth2DFromPointCloud(pclScan);