1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51
| import numpy as np import rospy from sensor_msgs.msg import PointCloud2 import sensor_msgs.point_cloud2 as pc2 import scipy.misc import os def point_cloud_2_birdseye(points): x_points = points[:, 0] y_points = points[:, 1] z_points = points[:, 2]
f_filt = np.logical_and((x_points > -50), (x_points < 50)) # logical_and(逻辑与) s_filt = np.logical_and((y_points > -50), (y_points < 50)) filter = np.logical_and(f_filt, s_filt) indices = np.argwhere(filter) # 筛选符合范围的points # 返回符合filter条件的位置索引,即第几个位置
x_points = x_points[indices] y_points = y_points[indices] z_points = z_points[indices]
x_img = (-y_points*10).astype(np.int32)+500 # 转换数组的数据类型 # 点云数据通常是浮点数,而图像数据通常是整数,所以要float映射到int y_img = (-x_points *10).astype(np.int32)+500
pixel_values = np.clip(z_points,-2,2) # numpy.clip(a, a_min, a_max, out=None) # 将数组中的元素限制在-2和2之间,大于2的就使得它等于2,小于-2,的就使得它等于-2 pixel_values = ((pixel_values +2) / 4.0) * 255
im = np.zeros([1001, 1001], dtype=np.uint8) im[y_img, x_img] = pixel_values return im
def callback(lidar): lidar = pc2.read_points(lidar) # 函数 point_cloud2.read_points(data, field_names=("x", "y", "z"), skip_nans=True) # 这个函数返回值是一个generator(python中的生成器,属于Iterator迭代器的一种) points = np.array(list(lidar)) # 如果需要一次获得全部点,可以用list()转换为列表 im = point_cloud_2_birdseye(points) scipy.misc.imsave('./lidar.png', im) # 将数组保存成图像 os._exit(0) # python无错误退出程序
def cloud_subscribe(): rospy.init_node('cloud_subscribe_node') rospy.Subscriber("/velodyne_points", PointCloud2, callback) rospy.spin() cloud_subscribe()
|