ros::Publisher pub; ros::Publisher planePub; typedef pcl::PointCloudpcl::PointXYZ PointCloud;

void cloudcb(const sensormsgs::PointCloud2ConstPtr& input) { // Container for original & filtered data pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); pcl::PCLPointCloud2 cloud_filtered;

// Convert to PCL data type pcl_conversions::toPCL(*input, *cloud);

// Convert to ROS data type sensor_msgs::PointCloud2 output;

// Publish the data pub.publish (output); ROS_INFO("Received Data");

pcl::PCLPointCloud2 pclpc2; pclconversions::toPCL(*input,pcl_pc2);

pcl::PointCloudpcl::PointXYZ::Ptr inputCloud(new pcl::PointCloudpcl::PointXYZ); pcl::fromPCLPointCloud2(pcl_pc2,*inputCloud);

pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients); pcl::PointIndices::Ptr inliers (new pcl::PointIndices); // Create the segmentation object pcl::SACSegmentationpcl::PointXYZ seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODELPLANE); seg.setMethodType (pcl::SACRANSAC); seg.setDistanceThreshold (0.01); seg.setInputCloud(inputCloud); seg.segment (*inliers, *coefficients);

sensormsgs::PointCloud2 cloudmsg;

if (inliers->indices.size () == 0) { PCL_ERROR ("Could not estimate a planar model for the given dataset."); return (-1); }

else {

std::cerr << "Model coefficients: " << coefficients->values[0] << " " << coefficients->values[1] << " " << coefficients->values[2] << " " << coefficients->values[3] << std::endl;

std::cerr << "Model inliers: " << inliers->indices.size () << std::endl; for (size_t i = 0; i < inliers->indices.size (); ++i) std::cerr << inliers->indices[i] << " " << cloud->planePoints[inliers->indices[i]].x << " " << cloud->planePoints[inliers->indices[i]].y << " " << cloud->planePoints[inliers->indices[i]].z << std::endl; // Create the filtering object pcl::ExtractIndicespcl::PointXYZ extract; PointCloud::Ptr planePoints(new PointCloud); // Extract the inliers extract.setInputCloud (inputCloud); extract.setIndices (inliers); extract.setNegative (false); extract.filter (*planePoints); std::cerr << "PointCloud representing the planar component: " << planePoints->width * planePoints->height << " data points." << std::endl;

planePoints->header.frame_id = "plane_frame";
planePoints->header.stamp = ros::Time::now().toNSec();
/*
pcl_conversions::fromPCL(planePoints, output);
planePub(planePoints);
pub(planePoints);
/*
for (size_t i = 0; i < inliers->indices.size (); ++i)
{
  std::cerr << inliers->indices[i] << "    " << inputCloud->points[inliers->indices[i]].x << " "<< inputCloud->points[inliers->indices[i]].y << " "<< inputCloud->points[inliers->indices[i]].z << std::endl;
}
*/

} }

int main (int argc, char** argv) {

// Initialize ROS ros::init (argc, argv, "planesegment"); ros::NodeHandle nh; std::string topic = nh.resolveName("pointcloud");; // Create a ROS subscriber for the input point cloud ros::Subscriber sub = nh.subscribe ("/softkineticcamera/depth/points", 1, cloudcb); pub = nh.advertise("/planepoints", 1); planePub = nh.advertise ("/pclplane_point", 1);

// Spin ros::spin (); }