/*! 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;
}
