/*! Processes the latest point cloud and gives back the resulting array of models. */ bool TabletopSegmentor::serviceCallback(TabletopSegmentation::Request &request, TabletopSegmentation::Response &response) { ros::Time start_time = ros::Time::now(); std::string topic = nh_.resolveName("cloud_in"); //ROS_INFO("Tabletop detection service called; waiting for a point_cloud2 on topic %s", topic.c_str()); sensor_msgs::PointCloud2::ConstPtr recent_cloud = ros::topic::waitForMessage<sensor_msgs::PointCloud2>(topic, nh_, ros::Duration(10.0)); pcl::PointCloud<Point>::Ptr cloud(new pcl::PointCloud<Point>); pcl::PointCloud<Point>::Ptr converted_cloud(new pcl::PointCloud<Point>); pcl::fromROSMsg(*recent_cloud,*cloud); if (!recent_cloud) { ROS_ERROR("Tabletop object detector: no point_cloud2 has been received"); response.result = response.NO_CLOUD_RECEIVED; return true; } if(convertFrame(cloud,converted_cloud)) { processCloud(converted_cloud,response); //ROS_INFO_STREAM("In total, segmentation took " << ros::Time::now() - start_time << " seconds"); return true; } else { response.result = response.OTHER_ERROR; //ROS_INFO_STREAM("In total, segmentation took " << ros::Time::now() - start_time << " seconds"); return true; } }
void TabletopSegmentor::cloudCallBack(const sensor_msgs::PointCloud2::ConstPtr cloud_in) { ros::Time start_time = ros::Time::now(); pcl::PointCloud<Point>::Ptr cloud(new pcl::PointCloud<Point>); pcl::PointCloud<Point>::Ptr converted_cloud(new pcl::PointCloud<Point>); pcl::fromROSMsg(*cloud_in,*cloud); convertFrame(cloud,converted_cloud); TabletopSegmentation::Response r; processCloud(converted_cloud,r); TabletopClusters c; c.table = r.table; c.clusters = r.clusters; c.result = r.result; cluster_pub.publish(c); }
pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_open_pcd_file(std::string file_path) { pcl::PointCloud <pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud <pcl::PointXYZRGB>); if ( pcl::io::loadPCDFile <pcl::PointXYZRGB> (file_path, *cloud) != 0 ) { std::cout << "Cloud reading failed. may be xyz cloud " << std::endl; pcl::PointCloud <pcl::PointXYZ>::Ptr xyz_cloud (new pcl::PointCloud <pcl::PointXYZ>); if(pcl::io::loadPCDFile<pcl::PointXYZ>(file_path, *xyz_cloud) != 0) { pcl::PointCloud <pcl::PointXYZRGB>::Ptr converted_cloud (new pcl::PointCloud <pcl::PointXYZRGB>); converted_cloud = open_point_cloud_xyz_to_xyzrgb(xyz_cloud); cloud = converted_cloud; } } return cloud; }