У меня есть код 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);
Код точно такой же, как в здесь .
Может ли это быть проблемой с кодом? Эти операторы не могут быть реализованы в функции обратного вызова .?