Euclidean Cluster Extraction

edit

I'm trying to extract clusters from point cloud. I'm doing it with help of tutorial:

PCL Tutorial

Everything works fine, even point clouds show if I save them in .pcd file, but when I try to view them in rviz nothing is seen. If I subscribe to appropriate topic no points are received. It says status: error, Transform: For frame []: Frame [] does not exist.

The funny thing is if I do: rostopic echo clusters I see that points are generated on that topic. Do I have to add any post processing to the extracted clusters to see them in rviz?

I'm using ubuntu, ros electric and perception_pcl_electric_unstable.

Here is some of my code:

    sensor_msgs::PointCloud2::Ptr clusters (new sensor_msgs::PointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    /* Creating the KdTree from input point cloud*/
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (input_cloud);

/* Here we are creating a vector of PointIndices, which contains the actual index
 * information in a vector<int>. The indices of each detected cluster are saved here.
 * Cluster_indices is a vector containing one instance of PointIndices for each detected 
 * cluster. Cluster_indices[0] contain all indices of the first cluster in input point cloud.
 */
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.06); 
ec.setMinClusterSize (30);
ec.setMaxClusterSize (600);
ec.setSearchMethod (tree);
ec.setInputCloud (input_cloud);
/* Extract the clusters out of pc and save indices in cluster_indices.*/
ec.extract (cluster_indices);

/* To separate each cluster out of the vector<PointIndices> we have to 
 * iterate through cluster_indices, create a new PointCloud for each 
 * entry and write all points of the current cluster in the PointCloud. 
 */

std::vector<pcl::PointIndices>::const_iterator it;
std::vector<int>::const_iterator pit;
for(it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
        for(pit = it->indices.begin(); pit != it->indices.end(); pit++) {
        //push_back: add a point to the end of the existing vector
                cloud_cluster->points.push_back(input_cloud->points[*pit]); 
        }

        //Merge current clusters to whole point cloud
    *clustered_cloud += *cloud_cluster;

  }

pcl::toROSMsg (*clustered_cloud , *clusters);
publish_clusters.publish (*clusters);