时间戳

首页 > 解决方案 > 在 PointCloud2 结构 C++/ROS/PCL 库中写入/访问点

c++ - 在 PointCloud2 结构 C++/ROS/PCL 库中写入/访问点

问题描述

我目前正在构建感知管道，并且无法读取我的点云结构的点。我正在将点云读取到 PointCloud2 结构中，并希望能够写出点云的系列点，或者至少可以访问它们，因此我找到了一种稍后将它们写入文件的方法。我正在使用的基本代码在这里：

void cloud_cropbox_cb(const sensor_msgs::PointCloud2ConstPtr &cloud_msg)
{
    // Container for original & filtered data
    pcl::PCLPointCloud2 *cloud = new pcl::PCLPointCloud2;
    pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
    pcl::PCLPointCloud2 cloud_filtered;

    // convert given message into PCL data type
    pcl_conversions::toPCL(*cloud_msg, *cloud);

我希望能够访问云中每个点的 x、y、z 元素，理想情况下只需将它们全部写入文本文件。我尝试过使用pcl::io::savePCDFileASCII()，但我认为这在这里不太适用。另一个注意事项是基本程序一直在运行，并且在手动关闭之前永远不会退出我不知道这是否与写入文件有关。任何有关使用哪些确切功能/我需要采取的步骤的帮助将不胜感激。

标签： c++file-iorospoint-cloud-librarypcl

解决方案

如果您真的想将消息的内容写入文件，您可以（在启动文件或命令行中）使用ros topic echo [args] > my_file.txt.

对于在 C++ 中兼容的方式，我建议阅读http://wiki.ros.org/pcl_ros，其中描述了：

pcl_ros 扩展了 ROS C++ 客户端库以支持使用 PCL 原生数据类型进行消息传递。只需将以下包含添加到您的 ROS 节点源代码中： #include <pcl_ros/point_cloud.h> 此标头允许您将 pcl::PointCloud 对象作为 ROS 消息发布和订阅。这些在 ROS 看来是 sensor_msgs/PointCloud2 消息，提供与不使用 PCL 的 ROS 节点的无缝互操作性。例如，您可以在其中一个节点中发布 pcl::PointCloud 并使用 Point Cloud2 显示在 rviz 中将其可视化。它通过连接到 roscpp 序列化基础设施来工作。

这意味着您可以在内部随心所欲地使用 PCL，这都是 ROS PointCloud2 消息。但是您必须在访问数据的消息/发布者/订阅者中声明要使用的 PCL 类型/接口。所以不要sensor_msgs/PointCloud2到处使用类型，而是到处使用pcl::Pointcloud<pcl::Point*>类型。

出版：

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
ros::Publisher pub = nh.advertise<PointCloud> ("points2", 1);
PointCloud::Ptr msg (new PointCloud);
msg->points.push_back (pcl::PointXYZ(1.0, 2.0, 3.0));
pub.publish (msg);

订阅：

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
ros::Subscriber sub = nh.subscribe<PointCloud>("points2", 1, callback);
void callback(const PointCloud::ConstPtr& msg){
  printf ("Cloud: width = %d, height = %d\n", msg->width, msg->height);
  BOOST_FOREACH (const pcl::PointXYZ& pt, msg->points)
  printf ("\t(%f, %f, %f)\n", pt.x, pt.y, pt.z);
}

http://wiki.ros.org/pcl_ros中的更多示例（或节点源代码的链接），http ://wiki.ros.org/pcl_ros/Tutorials 中有关其他管道内容的教程。

推荐阅读