Question

我正在尝试使用ROS和PCL_ROS裁剪PointCloud2，但是不幸的是我找不到我的错误。错误是，Rviz不会显示我裁剪的云，即使该主题已发布，我什至可以选择它。我什至尝试通过在云上进行迭代来调查数据（这也未显示在终端中）。我想知道你能不能帮我？

代码如下：

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/crop_box.h>
#include <sensor_msgs/point_cloud2_iterator.h>

 ros::Publisher pub;

 void
 cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
 {

     pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
     pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
     pcl::PCLPointCloud2 cloud_filtered;

     // Do data processing here...
     pcl_conversions::toPCL(*cloud_msg, *cloud);

     pcl::CropBox<pcl::PCLPointCloud2> boxFilter;
     boxFilter.setInputCloud(cloudPtr);
     boxFilter.setMin(Eigen::Vector4f(1.0, 1.0, 1.0, 0));
     boxFilter.setMax(Eigen::Vector4f(4.0, 4.0, 4.0, 0));
     boxFilter.setTranslation(Eigen::Vector3f(0.0, 0.0, 0.0));
     boxFilter.filter(cloud_filtered);
     // Convert to ROS data type

  sensor_msgs::PointCloud2 output;
  pcl_conversions::fromPCL(cloud_filtered, output);

     for (sensor_msgs::PointCloud2ConstIterator<float> it(output, "x"); it != it.end(); ++it) {
         // TODO: do something with the values of x, y, z

         std::cout <<  it[0] << "/ ";
         std::cout <<  it[1]<< "/ ";
         std::cout <<  it[2]<< "/ ";
         std::cout << std::endl;
     }

   // Publish the data.
   pub.publish (output);

 }

 int
 main (int argc, char** argv)
 {
   // Initialize ROS
   ros::init (argc, argv, "CROP");
   ros::NodeHandle nh;

   // Create a ROS subscriber for the input point cloud
   ros::Subscriber sub = nh.subscribe ("/kinect2/qhd/points", 1, cloud_cb);

   // Create a ROS publisher for the output point cloud
   pub = nh.advertise<sensor_msgs::PointCloud2> ("/Cloud_Cropped", 1);

   // Spin
   ros::spin ();
 }

PCL / ROS Cropbox什么都不显示？

0 个答案: