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

print sizes

Browse Source
This commit is contained in:
Oleg Sinyavskiy
2015-06-22 14:48:48 -07:00
parent a43def5dbe
commit 08f65901aa
1 changed files with 13 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+13 -10
View File
@@ -163,9 +163,7 @@ private:
ros::Duration process_duration = curtime - lasttime; ros::Duration process_duration = curtime - lasttime;
ros::Duration between_frames = curtime - this->_lastFrameTime; ros::Duration between_frames = curtime - this->_lastFrameTime;
this->_lastFrameTime = curtime; this->_lastFrameTime = curtime;
std::stringstream buffer;
buffer << "acloudsize=" << cloud->size() << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz";
ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str());
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
if(ground.get() && ground->size()) if(ground.get() && ground->size())
@@ -179,14 +177,19 @@ private:
pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud); pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
} }
if(maxFloorHeight_ > 0) std::stringstream buffer;
{ buffer << "cloud=" << cloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size();
pcl::PointCloud<pcl::PointXYZ>::Ptr flatObstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>); buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz";
flatObstaclesCloud = rtabmap::util3d::passThrough(groundCloud, "z", maxFloorHeight_, std::numeric_limits<int>::max()); ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str());
*obstaclesCloud += *flatObstaclesCloud;
groundCloud = rtabmap::util3d::passThrough(groundCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_); // if(maxFloorHeight_ > 0)
} // {
// pcl::PointCloud<pcl::PointXYZ>::Ptr flatObstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
// flatObstaclesCloud = rtabmap::util3d::passThrough(groundCloud, "z", maxFloorHeight_, std::numeric_limits<int>::max());
// *obstaclesCloud += *flatObstaclesCloud;
//
// groundCloud = rtabmap::util3d::passThrough(groundCloud, "z", std::numeric_limits<int>::min(), maxFloorHeight_);
// }
if(groundPub_.getNumSubscribers()) if(groundPub_.getNumSubscribers())
{ {