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

Question

我目前正在构建感知管道，但在读取点云结构的点时遇到困难。我正在将点云读入 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()

但我认为这在这里不太适用。附加说明是基本程序不断运行并且在手动关闭之前永远不会退出我不知道这是否与写入文件有关。任何有关我需要使用哪些确切功能/需要采取的步骤的帮助将不胜感激。

Answer 1

如果您确实想将消息的内容写入文件，您可以（在启动文件或命令行中）使用

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。