Не удалось выполнить код PCL в обратном вызове (ROS) - PullRequest
Новая
       56

Не удалось выполнить код PCL в обратном вызове (ROS)

0 голосов
/ 09 мая 2018

У меня есть код C ++ в формате ROS. Ниже приводится функция обратного вызова: 

void cloud_cb(const boost::shared_ptr<const sensor_msgs::PointCloud2>& input){
    ROS_INFO("callback");
    pcl::PCLPointCloud2 pcl_pc2;
    pcl_conversions::toPCL(*input,pcl_pc2);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2(pcl_pc2,*cloud);
    if(cloud->size() > 300) {
        ROS_INFO("if loop");
        pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree (new pcl::search::KdTree<pcl::PointXYZ>);
        ROS_INFO("1");
        pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimation; 
        ROS_INFO("2");
        normal_estimation.setInputCloud (cloud);
        ROS_INFO("3");
        normal_estimation.setSearchMethod (kdtree);
        ROS_INFO("4");
        pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud< pcl::Normal>);
        ROS_INFO("5");
        normal_estimation.setRadiusSearch (0.03);
        ROS_INFO("6");
        normal_estimation.compute (*normals);
        ROS_INFO("7");
        PFHShapeReco<pcl::PointXYZ> pfh;
        ROS_INFO("8");
        pfh.ComputePFH(0.5, cloud, normals);
        ROS_INFO("9");
        std_msgs::Float32MultiArray arr;
        for(int i = 0; i < 625; i++){
            float f = pfh.GetObjectModel()[i];
            arr.data.push_back(f);
        }
        hist.publish (arr);
    }
}

Обратите внимание, что я добавил различные сообщения ROS_INFO в качестве контрольных точек. Проблема здесь заключается в задержке между контрольной точкой 6 и 7. Следующее утверждение требует много времени для реализации,

normal_esvaluation.compute (* normals);

Код точно такой же, как в здесь .

Может ли это быть проблемой с кодом? Эти операторы не могут быть реализованы в функции обратного вызова .?

...