void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
{
if(groundPub_.getNumSubscribers() || obstaclesPub_.getNumSubscribers())
{
rtabmap::Transform localTransform;
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
{
ROS_WARN("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_WARN("%s",ex.what());
return;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloudMsg, *cloud);
pcl::IndicesPtr ground, obstacles;
if(cloud->size())
{
cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(cloud, localTransform);
if(maxObstaclesHeight_ > 0)
{
cloud = rtabmap::util3d::passThrough<pcl::PointXYZ>(cloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
}
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud,
ground, obstacles, normalEstimationRadius_, groundNormalAngle_, minClusterSize_);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
if(groundPub_.getNumSubscribers() && ground->size())
{
pcl::copyPointCloud(*cloud, *ground, *groundCloud);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
if(obstaclesPub_.getNumSubscribers() && obstacles->size())
{
pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
}
if(groundPub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(*groundCloud, 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(*obstaclesCloud, rosCloud);
rosCloud.header.stamp = cloudMsg->header.stamp;
rosCloud.header.frame_id = frameId_;
//publish the message
obstaclesPub_.publish(rosCloud);
}
}
}
void callback(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
{
ros::Time time = ros::Time::now();
if (groundPub_.getNumSubscribers() == 0 && obstaclesPub_.getNumSubscribers() == 0)
{
// no one wants the results
return;
}
rtabmap::Transform localTransform;
try
{
if(waitForTransform_)
{
if(!tfListener_.waitForTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, ros::Duration(1)))
{
ROS_ERROR("Could not get transform from %s to %s after 1 second!", frameId_.c_str(), cloudMsg->header.frame_id.c_str());
return;
}
}
tf::StampedTransform tmp;
tfListener_.lookupTransform(frameId_, cloudMsg->header.frame_id, cloudMsg->header.stamp, tmp);
localTransform = rtabmap_ros::transformFromTF(tmp);
}
catch(tf::TransformException & ex)
{
ROS_ERROR("%s",ex.what());
return;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*cloudMsg, *originalCloud);
//Common variables for all strategies
pcl::IndicesPtr ground, obstacles;
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
if(originalCloud->size())
{
originalCloud = rtabmap::util3d::transformPointCloud(originalCloud, localTransform);
if(maxObstaclesHeight_ > 0)
{
originalCloud = rtabmap::util3d::passThrough(originalCloud, "z", std::numeric_limits<int>::min(), maxObstaclesHeight_);
}
if(originalCloud->size())
{
if(!optimizeForCloseObjects_)
{
// This is the default strategy
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
originalCloud,
ground,
obstacles,
normalEstimationRadius_,
groundNormalAngle_,
minClusterSize_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
{
pcl::copyPointCloud(*originalCloud, *ground, *groundCloud);
}
if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
{
pcl::copyPointCloud(*originalCloud, *obstacles, *obstaclesCloud);
}
}
else
{
// in this case optimizeForCloseObject_ is true:
// we divide the floor point cloud into two subsections, one for all potential floor points up to 1m
// one for potential floor points further away than 1m.
// For the points at closer range, we use a smaller normal estimation radius and ground normal angle,
// which allows to detect smaller objects, without increasing the number of false positive.
// For all other points, we use a bigger normal estimation radius (* 3.) and tolerance for the
// grond normal angle (* 2.).
pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_near = rtabmap::util3d::passThrough(originalCloud, "x", std::numeric_limits<int>::min(), 1.);
pcl::PointCloud<pcl::PointXYZ>::Ptr originalCloud_far = rtabmap::util3d::passThrough(originalCloud, "x", 1., std::numeric_limits<int>::max());
// Part 1: segment floor and obstacles near the robot
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
originalCloud_near,
ground,
obstacles,
normalEstimationRadius_,
groundNormalAngle_,
minClusterSize_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
{
pcl::copyPointCloud(*originalCloud_near, *ground, *groundCloud);
ground->clear();
}
if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
{
pcl::copyPointCloud(*originalCloud_near, *obstacles, *obstaclesCloud);
obstacles->clear();
}
// Part 2: segment floor and obstacles far from the robot
rtabmap::util3d::segmentObstaclesFromGround<pcl::PointXYZ>(
originalCloud_far,
ground,
obstacles,
3.*normalEstimationRadius_,
2.*groundNormalAngle_,
minClusterSize_);
if(groundPub_.getNumSubscribers() && ground.get() && ground->size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud2 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*originalCloud_far, *ground, *groundCloud2);
*groundCloud += *groundCloud2;
}
if(obstaclesPub_.getNumSubscribers() && obstacles.get() && obstacles->size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr obstacles2(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*originalCloud_far, *obstacles, *obstacles2);
*obstaclesCloud += *obstacles2;
}
}
}
}
if(groundPub_.getNumSubscribers())
{
sensor_msgs::PointCloud2 rosCloud;
pcl::toROSMsg(*groundCloud, 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(*obstaclesCloud, rosCloud);
rosCloud.header.stamp = cloudMsg->header.stamp;
rosCloud.header.frame_id = frameId_;
//publish the message
obstaclesPub_.publish(rosCloud);
}
//ROS_INFO("Obstacles segmentation time = %f s", (ros::Time::now() - time).toSec());
}
void occupancy2DFromCloud3D(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
cv::Mat & ground,
cv::Mat & obstacles,
float cellSize,
float groundNormalAngle,
int minClusterSize)
{
if(cloud->size() == 0)
{
return;
}
pcl::IndicesPtr groundIndices, obstaclesIndices;
segmentObstaclesFromGround<PointT>(
cloud,
indices,
groundIndices,
obstaclesIndices,
cellSize,
groundNormalAngle,
minClusterSize);
pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
if(groundIndices->size())
{
pcl::copyPointCloud(*cloud, *groundIndices, *groundCloud);
//project on XY plane
util3d::projectCloudOnXYPlane(groundCloud);
//voxelize to grid cell size
groundCloud = util3d::voxelize(groundCloud, cellSize);
}
if(obstaclesIndices->size())
{
pcl::copyPointCloud(*cloud, *obstaclesIndices, *obstaclesCloud);
//project on XY plane
util3d::projectCloudOnXYPlane(obstaclesCloud);
//voxelize to grid cell size
obstaclesCloud = util3d::voxelize(obstaclesCloud, cellSize);
}
ground = cv::Mat();
if(groundCloud->size())
{
ground = cv::Mat((int)groundCloud->size(), 1, CV_32FC2);
for(unsigned int i=0;i<groundCloud->size(); ++i)
{
ground.at<cv::Vec2f>(i)[0] = groundCloud->at(i).x;
ground.at<cv::Vec2f>(i)[1] = groundCloud->at(i).y;
}
}
obstacles = cv::Mat();
if(obstaclesCloud->size())
{
obstacles = cv::Mat((int)obstaclesCloud->size(), 1, CV_32FC2);
for(unsigned int i=0;i<obstaclesCloud->size(); ++i)
{
obstacles.at<cv::Vec2f>(i)[0] = obstaclesCloud->at(i).x;
obstacles.at<cv::Vec2f>(i)[1] = obstaclesCloud->at(i).y;
}
}
}
