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; } } }