How to send cluster point point in separated node

asked 2020-05-26 06:36:04 -0500

alfan gravatar image

Hi i'm new in pointcloud library and ROS.Im using ROS Kinetic on Ubuntu 18.04 and pcl1-9-1 lib. I'm trying to show clustering result point on rviz or pcl viewer, and then show nothing. And i realize that my data show nothing too when i subcsribe and cout that. Hopefully can help my problem, thanks

This is my code for clustering and send node

void cloudReceive(const sensor_msgs::PointCloud2ConstPtr& inputMsg){
mutex_lock.lock();
pcl::fromROSMsg(*inputMsg, *inputCloud);
cout<<inputCloud<<endl;
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud(inputCloud);

std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;

ec.setClusterTolerance(0.03);//2cm
ec.setMinClusterSize(200);//min points
ec.setMaxClusterSize(1000);//max points
ec.setSearchMethod(tree);
ec.setInputCloud(inputCloud);
ec.extract(cluster_indices);

if(cluster_indices.size() > 0){
std::vector<pcl::PointIndices>::const_iterator it;

int i = 0;

for (it = cluster_indices.begin(); it != cluster_indices.end(); ++it){
    if(i >= 10)
        break;

    cloud_cluster[i]->points.clear();
    std::vector<int>::const_iterator idx_it;

    for (idx_it = it->indices.begin(); idx_it != it->indices.end(); idx_it++)
        cloud_cluster[i]->points.push_back(inputCloud->points[*idx_it]);

    cloud_cluster[i]->width = cloud_cluster[i]->points.size();
    // cloud_cluster[i]->height = 1;
    // cloud_cluster[i]->is_dense = true;

    cout<<"PointCloud representing the Cluster: " << cloud_cluster[i]->points.size() << " data points"<<endl;
    std::stringstream ss;
    ss<<"cobaa_pipecom2_cluster_"<< i << ".pcd";
    writer.write<pcl::PointXYZRGB> (ss.str(), *cloud_cluster[i], false);

    pcl::toROSMsg(*cloud_cluster[i], outputMsg);
    // cout<<"data = "<< outputMsg <<endl;
    cloud_cluster[i]->header.frame_id = FRAME_ID;
    pclpub[i++].publish(outputMsg);

    // i++;
}

And this is the code for my main

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

for (int z = 0; z < 10; z++) {
// std::cout << " - clustering/" << z << std::endl;

cloud_cluster[z] = pcl::PointCloud<pcl::PointXYZRGB>::Ptr(new pcl::PointCloud<pcl::PointXYZRGB>);
cloud_cluster[z]->height = 1;
cloud_cluster[z]->is_dense = true;
// cloud_cluster[z]->header.frame_id = FRAME_ID;
}

ros::init(argc,argv,"clustering");
ros::NodeHandlePtr nh(new ros::NodeHandle());
pclsub = nh->subscribe("/pclsegmen",1,cloudReceive);

std::string pub_str("clustering/0");

for (int z = 0; z < 10; z++) {
pub_str[11] = z + 48;//48=0(ASCII)
// z++;
pclpub[z] = nh->advertise <sensor_msgs::PointCloud2> (pub_str, 1);
}


ros::spin();
edit retag flag offensive close merge delete