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

tming of the whole function

Browse Source
This commit is contained in:
Oleg Sinyavskiy
2015-06-22 15:49:13 -07:00
parent 33612c7b12
commit 3cd48d670a
1 changed files with 11 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+11 -10
View File
@@ -155,6 +155,7 @@ private:
ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_); ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
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())
{ {
@@ -171,16 +172,6 @@ private:
*obstaclesCloud += *obstaclesNearFloorCloud; *obstaclesCloud += *obstaclesNearFloorCloud;
} }
ros::Time curtime = ros::Time::now();
ros::Duration process_duration = curtime - lasttime;
ros::Duration between_frames = curtime - this->_lastFrameTime;
this->_lastFrameTime = curtime;
std::stringstream buffer;
buffer << "cloud=" << originalCloud->size() << " hypothetical ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size();
buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz";
ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str());
if(groundPub_.getNumSubscribers()) if(groundPub_.getNumSubscribers())
{ {
sensor_msgs::PointCloud2 rosCloud; sensor_msgs::PointCloud2 rosCloud;
@@ -203,6 +194,16 @@ private:
obstaclesPub_.publish(rosCloud); obstaclesPub_.publish(rosCloud);
} }
ros::Time curtime = ros::Time::now();
ros::Duration process_duration = curtime - lasttime;
ros::Duration between_frames = curtime - this->_lastFrameTime;
this->_lastFrameTime = curtime;
std::stringstream buffer;
buffer << "cloud=" << originalCloud->size() << " ground=" << hypotheticalGroundCloud->size() << " floor=" << ground->size() << " obst=" << obstacles->size();
buffer << " t=" << process_duration.toSec() << "s; " << (1./between_frames.toSec()) << "Hz";
ROS_ERROR("3%s: %s", this->getName().c_str(), buffer.str().c_str());
} }
private: private: