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

Publish even when pointcloud is null, otherwise system hangs

Browse Source
This commit is contained in:
JRombouts
2015-07-08 21:36:06 -07:00
parent 37a53d91c2
commit 59986a8066
1 changed files with 24 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/obstacles_detection.cpp
+24
View File
@@ -140,11 +140,35 @@ private:
pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloudMsg, *originalCloud); pcl::fromROSMsg(*cloudMsg, *originalCloud);
if(originalCloud->size() == 0) if(originalCloud->size() == 0)
{ {
ROS_ERROR("Recieved empty point cloud!"); ROS_ERROR("Recieved empty point cloud!");
if(groundPub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(*originalCloud, rosCloud);
rosCloud.header.stamp = cloudMsg->header.stamp;
rosCloud.header.frame_id = frameId_;
//publish the message
groundPub_.publish(rosCloud);
}
if(obstaclesPub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(*originalCloud, rosCloud);
rosCloud.header.stamp = cloudMsg->header.stamp;
rosCloud.header.frame_id = frameId_;
//publish the message
obstaclesPub_.publish(rosCloud);
}
return; return;
} }
originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform); originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);