代码收藏家 python将话题中的点云数据保存下来
全部代码
# save_pcd.py
import rospy
import pcl
import pcl.pcl_visualization
from sensor_msgs.msg import PointCloud2
from pcl import PointCloud
import ros_numpy
def callback(data):
# 将 PointCloud2 数据转换为 PCL 点云格式
pc_data = ros_numpy.numpify(data)
cloud = pcl.PointCloud()
cloud.from_array(pc_data[['x', 'y', 'z']].view(np.float32))
# 保存为 PCD 文件
pcl.save(cloud, 'output.pcd')
rospy.loginfo("PointCloud saved to output.pcd")
def listener():
rospy.init_node('pcd_saver', anonymous=True)
rospy.Subscriber('/points_raw', PointCloud2, callback)
rospy.spin()
if __name__ == '__main__':
listener()
讲解
这段代码的主要目的是监听 ROS 话题 /points_raw，接收点云数据（PointCloud2 消息类型），将其转换为 PCL（Point Cloud Library）格式的点云，并将该点云保存为 .pcd 文件。具体的功能实现如下：
1. 导入必要的库
import rospy
import pcl
import pcl.pcl_visualization
from sensor_msgs.msg import PointCloud2
from pcl import PointCloud
import ros_numpy
PointCloud2 是 ROS 中用于表示点云数据的标准消息类型。PointCloud2）转换为 NumPy 数组，以便进一步处理。2. 回调函数：callback
def callback(data):
# 将 PointCloud2 数据转换为 PCL 点云格式
pc_data = ros_numpy.numpify(data)
cloud = pcl.PointCloud()
cloud.from_array(pc_data[['x', 'y', 'z']].view(np.float32))
# 保存为 PCD 文件
pcl.save(cloud, 'output.pcd')
rospy.loginfo("PointCloud saved to output.pcd")
callback(data)：当从话题 /points_raw 接收到 PointCloud2 消息时，这个回调函数会被触发。data 参数包含了从传感器接收到的点云数据。
ros_numpy.numpify(data)：这个函数将 ROS 的 PointCloud2 消息转换为一个 NumPy 数组，便于后续操作。data 是原始的 ROS 消息，而 numpify 会将其转换为 NumPy 数组（结构化数组），其字段包括 x、y、z 等坐标信息。
pc_data 会是一个包含 x、y、z 坐标的结构化 NumPy 数组，其中 x, y, z 分别表示点云中每个点的空间坐标。
cloud = pcl.PointCloud()：创建一个空的 PCL 点云对象。
cloud.from_array(pc_data[[‘x’, ‘y’, ‘z’]].view(np.float32))：将 NumPy 数组中的 x, y, z 三个坐标字段提取出来，并转换为 float32 类型，最后加载到 cloud（PCL 点云对象）中。
pcl.save(cloud, ‘output.pcd’)：将 PCL 点云保存到本地文件 output.pcd 中，.pcd 是点云数据的常见文件格式。
rospy.loginfo(“PointCloud saved to output.pcd”)：日志打印，告知用户点云数据已成功保存。
3. listener 函数
def listener():
rospy.init_node('pcd_saver', anonymous=True)
rospy.Subscriber('/points_raw', PointCloud2, callback)
rospy.spin()
rospy.init_node('pcd_saver', anonymous=True)：初始化一个 ROS 节点，节点名称为 pcd_saver，anonymous=True 会确保每次运行时节点名称是唯一的。rospy.Subscriber('/points_raw', PointCloud2, callback)：订阅话题 /points_raw，这个话题传递的是 PointCloud2 消息类型的数据。每当收到新的点云数据时，会调用 callback 函数来处理数据。rospy.spin()：阻塞当前线程，保持节点的运行状态直到手动终止。此时，ROS 会持续监听订阅的消息并调用回调函数。4. __main__ 入口
if __name__ == '__main__':
listener()
这一部分是 Python 程序的标准入口点，表示当脚本作为主程序运行时，调用 listener() 函数来启动节点并开始订阅话题。
总结
这段代码的功能可以概括为：
- 初始化一个 ROS 节点。
- 订阅一个名为
/points_raw的 ROS 话题，获取PointCloud2消息。 - 将收到的点云数据（
PointCloud2）转换为 PCL 格式。 - 将转换后的点云数据保存为
.pcd文件。 - 通过日志输出保存成功的提示信息。
需要注意的地方：
pcl 或 ros_numpy 库，可能需要安装它们：
sudo apt-get install python-pcl
pip install ros_numpy
/points_raw 话题正确发布，并且发布的消息类型是 PointCloud2，否则该脚本无法正常工作。作者：knighthood2001
