i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57: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/nodelets/obstacles_detection.cpp
+11 -5
View File
@@ -68,7 +68,8 @@ public:
normalEstimationRadius_(0.05),
groundNormalAngle_(M_PI_4),
minClusterSize_(20),
maxObstaclesHeight_(0)
maxObstaclesHeight_(0),
waitForTransform_(false)
{}
virtual ~ObstaclesDetection()
@@ -87,6 +88,7 @@ private:
pnh.param("ground_normal_angle", groundNormalAngle_, groundNormalAngle_);
pnh.param("min_cluster_size", minClusterSize_, minClusterSize_);
pnh.param("max_obstacles_height", maxObstaclesHeight_, maxObstaclesHeight_);
pnh.param("wait_for_transform", waitForTransform_, waitForTransform_);
cloudSub_ = nh.subscribe("cloud", 1, &ObstaclesDetection::callback, this);
@@ -103,12 +105,15 @@ private:
Transform localTransform;
try
{
tf::StampedTransform tmp;
if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
if(waitForTransform_)
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
return;
if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
localTransform = transformFromTF(tmp);
}
@@ -174,6 +179,7 @@ private:
double groundNormalAngle_;
int minClusterSize_;
double maxObstaclesHeight_;
bool waitForTransform_;
tf::TransformListener tfListener_;