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

Comment for debugging

Browse Source
This commit is contained in:
Jean-Baptiste Passot
2015-07-07 17:19:55 -07:00
parent 568ce4ebc1
commit 5e4ededf7a
1 changed files with 11 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+11
View File
@@ -114,6 +114,9 @@ private:
return; return;
} }
ROS_ERROR("1111111111111111111111");
rtabmap::Transform localTransform; rtabmap::Transform localTransform;
try try
{ {
@@ -125,6 +128,9 @@ private:
return; return;
} }
} }
ROS_ERROR("2222222222222222222222222222222");
tf::StampedTransform tmp; tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp); tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp); localTransform = rtabmap_ros::transformFromTF(tmp);
@@ -145,6 +151,8 @@ private:
} }
originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
ROS_ERROR("3333333333333333333333333");
///////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////
pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr hypotheticalGroundCloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -159,6 +167,8 @@ private:
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
if (!simpleSegmentation_){ if (!simpleSegmentation_){
ROS_ERROR("44444444444444444444444444444");
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud, rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud,
ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
@@ -174,6 +184,7 @@ private:
} }
} }
else{ else{
ROS_ERROR("555555555555555555555555");
groundCloud = hypotheticalGroundCloud; groundCloud = hypotheticalGroundCloud;
} }