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

Added rgbdslam_datasets.launch example to run RGBD SLAM datasets with RTAB-Map. Added "wait_to_transform" parameter (bool) to all nodes calling lookUpTransform()

Browse Source
This commit is contained in:
Mathieu Labbe
2014-12-03 22:35:43 -05:00
parent cbdcd855d8
commit aa8aa24e5d
15 changed files with 693 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/GuiWrapper.cpp
+29
View File
@@ -62,6 +62,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
app_(0),
mainWindow_(0),
frameId_("base_link"),
waitForTransform_(false),
cameraNodeName_("")
{
ros::NodeHandle nh;
@@ -103,6 +104,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
pnh.param("subscribe_depth", subscribeDepth, subscribeDepth);
pnh.param("subscribe_laserScan", subscribeLaserScan, subscribeLaserScan);
pnh.param("queue_size", queueSize, queueSize);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap/camera when pausing the process
this->setupCallbacks(subscribeDepth, subscribeLaserScan, queueSize);
@@ -493,6 +495,15 @@ void GuiWrapper::depthCallback(
Transform localTransform;
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
@@ -535,6 +546,15 @@ void GuiWrapper::scanCallback(
// TF ready?
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), scanMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, scanMsg->header.frame_id, scanMsg->header.stamp, tmp);
}
@@ -585,6 +605,15 @@ void GuiWrapper::depthScanCallback(
laser_geometry::LaserProjection projection;
projection.transformLaserScanToPointCloud(frameId_, *scanMsg, scanOut, tfListener_);
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), depthMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, depthMsg->header.frame_id, depthMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);