Как отправить кластер в отдельный узел ros pcl - PullRequest
Новая
       86

Как отправить кластер в отдельный узел ros pcl

0 голосов
/ 26 мая 2020

Привет, я новичок в библиотеке pointcloud. Я пытаюсь показать точку результата кластеризации в программе просмотра rviz или pcl, а затем ничего не показываю. И я понимаю, что мои данные тоже ничего не показывают, когда я подписываюсь и продаю это. Надеюсь, это поможет моей проблеме, спасибо

Это мой код для кластеризации и отправки узла 

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++;
    }
}
else
   ROS_INFO_STREAM("0 clusters extracted\n");

}

А этот основной 

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);
}

// pclpub = nh->advertise<sensor_msgs::PointCloud2>("/pclcluster",1);
ros::spin();

}

Ответы [ 2 ]

0 голосов
/ 01 июля 2020

Решение: я изменил номер ASCII на lexical_cast. Спасибо за ответ, надеюсь, это поможет другим 

    for (int z = 0; z < CLOUD_QTD; z++) {
    // pub_str[11] = z + 48;
    std::string topicName = "/pclcluster/" + boost::lexical_cast<std::string>(z);
    global::pub[z] = n.advertise <sensor_msgs::PointCloud2> (topicName, 1);
}
0 голосов
/ 27 мая 2020

Это не точный ответ, но я думаю, что он решит вашу проблему и может облегчить вашу отладку. RViz может напрямую подписаться на опубликованное облако точек, которое, как я предполагаю, вы пытаетесь увидеть в обратном вызове cloud_receive. Если вы установите для фрейма тот фрейм, в котором он публикуется, и добавите его из доступных тем, вы должны увидеть точки. (Это проще, чем пытаться ретранслировать его как разные темы).

Кроме того, я рекомендую посмотреть на инструмент командной строки rostopic. Вы можете сделать rostopic list, чтобы проверить, публикуется ли он, rostopic bw, чтобы увидеть, действительно ли он публикует ожидаемый объем данных (например, байты против килобайт против мегабайт), rostopic hz, чтобы узнать, как часто (если вообще) он публикуется, и (кратко) rostopic echo, чтобы посмотреть на сами данные. (Я предполагаю из вашего вопроса, что это больше проблема с данными, поступающими в ваш узел).

Если у вас проблемы, то не с данными, поступающими в узел, или с визуализацией данных облака точек в в общем, но с преобразованными данными, которые должны исходить из узла, я бы проверил, что кластеризация работает, и уменьшил бы ваш код еще больше, чтобы просто иметь 1 издатель publi sh что-то. Возможно, вы делаете что-то странное. Например, испортил ваши указатели. Вы также можете включить более строгие предупреждения компиляции для своего узла с помощью -Wall -Wextra -Werror или пошагово выполнить его через gdb (launch-prefix="xterm -e gdb --args").

...