ros python读取深度相机中的点云数据sensor_msgs.PointCloud2,获取点云中的XYZ坐标

用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的理解,可以参考下面这篇博文:

https://www.cnblogs.com/wj-1314/p/8490822.html

评论 15
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值