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

Comment for debugging

Browse Source
This commit is contained in:
Jean-Baptiste Passot
2015-07-07 17:53:14 -07:00
parent 0b0406723f
commit 04beb41c48
1 changed files with 12 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+12 -18
View File
@@ -126,7 +126,6 @@ private:
} }
} }
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);
@@ -148,38 +147,38 @@ 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>);
hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
ROS_ERROR("AAAa3333333333333333333333333");
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
ROS_ERROR("BBBb3333333333333333333333333");
ros::Time lasttime = ros::Time::now();
pcl::IndicesPtr ground, obstacles; pcl::IndicesPtr ground, obstacles;
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
ros::Time lasttime = ros::Time::now();
if (!simpleSegmentation_){ if (!simpleSegmentation_){
ROS_ERROR("44444444444444444444444444444");
ROS_ERROR("1-1");
hypotheticalGroundCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
ROS_ERROR("1-2");
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud, rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(hypotheticalGroundCloud,
ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
ROS_ERROR("1-3");
if(ground.get() && ground->size()) if(ground.get() && ground->size())
{ {
pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud); pcl::copyPointCloud(*hypotheticalGroundCloud, *ground, *groundCloud);
} }
ROS_ERROR("1-4");
obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_);
ROS_ERROR("RRR 44444444444444444444444444444"); ROS_ERROR("1-5");
if(obstacles.get() && obstacles->size()) if(obstacles.get() && obstacles->size())
{ {
@@ -191,12 +190,7 @@ private:
} }
else{ else{
ROS_ERROR("555555555555555555555555");
obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_); obstaclesCloud = rtabmap::util3d::passThrough(originalCloud, "z", maxFloorHeight_, maxObstaclesHeight_);
ROS_ERROR("RRR 555555555555555555555555");
groundCloud = hypotheticalGroundCloud;
} }