I can't visulaize the segmintation in Rviz this is my code [closed]

edit

ros::Publisher pub; ros::Publisher planePub; typedef pcl::PointCloud<pcl::pointxyz> PointCloud;

void cloud_cb(const sensor_msgs::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 pcl_pc2; pcl_conversions::toPCL(*input,pcl_pc2);

pcl::PointCloud<pcl::pointxyz>::Ptr inputCloud(new pcl::PointCloud<pcl::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::SACSegmentation<pcl::pointxyz> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold (0.01); seg.setInputCloud(inputCloud); seg.segment (*inliers, *coefficients);

sensor_msgs::PointCloud2 cloud_msg;

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::ExtractIndices<pcl::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, "plane_segment"); ros::NodeHandle nh; std::string topic = nh.resolveName("point_cloud");; // Create a ROS subscriber for the input point cloud ros::Subscriber sub = nh.subscribe ("/softkinetic_camera/depth/points", 1, cloud_cb); pub = nh.advertise<sensor_msgs::pointcloud2>("/plane_points", 1); planePub = nh.advertise<pointcloud> ("/pcl_plane_point", 1);

// Spin ros::spin (); }