Question

我有一个ROS格式的C ++代码。以下是回调函数：

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_estimation.compute（* normals）;

代码与here中的代码完全相同。

这可能是代码的问题吗？这些语句是否无法在回调函数中实现。？

无法在回调（ROS）中执行PCL代码

0 个答案: