ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

Unable to view to the clustering results in rviz.

asked 2016-01-27 14:42:05 -0600

updated 2016-01-27 14:45:10 -0600

Hello, I am trying to perform euclidean clustering in ROS. I can generate data and also store them .pcd files. But i wish to see the output of the file in rviz which I am unable to. It throws the following warning:

[WARN] [1426601373.806774699]: Invalid argument passed to canTransform argument source_frame in tf2 frame_ids cannot be empty

My code looks like this:

ros::Publisher pub;

void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input){

sensor_msgs::PointCloud2::Ptr clusters (new sensor_msgs::PointCloud2);  
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr clustered_cloud (new pcl::PointCloud<pcl::PointXYZ>);   

std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl;

// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size ()  << " data points." << std::endl;

// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);

int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
    // Segment the largest planar component from the remaining cloud
    seg.setInputCloud (cloud_filtered);
        seg.segment (*inliers, *coefficients);
        if (inliers->indices.size () == 0)
     {
            std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
            break;
         }

    // Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers);
    extract.setNegative (false);

    // Get the points associated with the planar surface
    extract.filter (*cloud_plane);
    std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;

    // Remove the planar inliers, extract the rest
    extract.setNegative (true);
    extract.filter (*cloud_f);
    *cloud_filtered = *cloud_f;
}

// Creating the KdTree object for the search method of the extraction
 pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);

std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (10);
ec.setMaxClusterSize (2500);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
std::vector<pcl::PointIndices>::const_iterator it;
std::vector<int>::const_iterator pit;

 int j = 0; 
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(cloud_filtered->points[*pit]); 

        cloud_cluster->width = cloud_cluster->points.size ();
            cloud_cluster->height = 1;
            cloud_cluster->is_dense = true;


        std::stringstream ss;
            ss << "cloud_cluster_" << j << ".pcd";
            writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false ...
(more)
edit retag flag offensive close merge delete

Comments

I haven't looked at this too closely, but I'd try making sure the header field of the clusters cloud matches the header field of the input cloud. Is the conversion to PCL and back to PointCloud2 properly preserving the frame information?

jarvisschultz gravatar image jarvisschultz  ( 2016-01-27 17:53:07 -0600 )edit

Thanks jarvis, I have done so. Its solved.

blackmamba591 gravatar image blackmamba591  ( 2016-01-27 17:55:56 -0600 )edit

I just came back here to tell you that I was convinced that was the problem... glad to hear that it was an easy fix!

jarvisschultz gravatar image jarvisschultz  ( 2016-01-27 18:08:10 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2016-01-27 17:57:32 -0600

updated 2016-01-27 17:58:50 -0600

frame_id of the headerof the pointcloud-message needs to be set.

clusters->header.frame_id = "/camera_depth_frame";
clusters->header.stamp=ros::Time::now();
pub.publish (*clusters);

This will solve the problem.

edit flag offensive delete link more
0

answered 2020-05-26 06:26:21 -0600

alfan gravatar image

can you send how to advertise and publish code? thank you before

edit flag offensive delete link more

Question Tools

1 follower

Stats

Asked: 2016-01-27 14:42:05 -0600

Seen: 1,720 times

Last updated: Jan 27 '16