用subscriber接收深度相机topic下的点云数据,获取点云中的XYZ坐标
msg数据格式是sensor_msgs.PointCloud2
这是一个一维或二维的数组,数据经过处理,无法直接读取点坐标XYZ信息,需要一步读取操作。
我的深度相机发出的topic的名字是/my_camera/depth/points
#! /usr/bin/env python
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2
import rospy
import time
def callback_pointcloud(data):
assert isinstance(data, PointCloud2)
gen = point_cloud2.read_points(data, field_names=("x", "y", "z"), skip_nans=True)
time.sleep(1)
print type(gen)
for p in gen:
print " x : %.3f y: %.3f z: %.3f" %(p[0],p[1],p[2])
def main():
rospy.init_node('pcl_listener', anonymous=True)
rospy.Subscriber('/my_camera/depth/points', PointCloud2, callback_pointcloud)
rospy.spin()
if __name__ == "__main__":
main()
关键部分:
函数 point_cloud2.read_points(data, field_names=("x", "y", "z"), skip_nans=True)
这个函数返回值是一个generator(python中的生成器,属于Iterator迭代器的一种)。
如果需要一次获得全部点,可以用list()转换为列表。
对于python中的generator和iterator的理解,可以参考下面这篇博文: