激光雷达数据结构
1
| rosmsg show sensor_msgs/LaserScan
|
可以看到ros输出的激光雷达数据结构:
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
| std_msgs/Header header # Header也是一个结构体,包含了seq,stamp,frame_id,其中seq # 指的是扫描顺序增加的id,stamp包含了开始扫描的时间和与开始扫 # 描的时间差,frame_id是扫描的参考系名称.注意扫描是逆时针从正前方开始扫描的. uint32 seq time stamp string frame_id # frame在ROS中作用至关重要,消息将和tf绑定才可以读取数据, float32 angle_min # 开始扫描的角度(rad) float32 angle_max # 结束扫描的角度(rad) float32 angle_increment # 每一次扫描增加的角度(rad)
float32 time_increment # 测量的时间间隔(second) float32 scan_time # 扫描的时间间隔(second)
float32 range_min # 距离最小值(m) float32 range_max # 距离最大值(m)
float32[] ranges # 距离数组(m) (长度360) float32[] intensities # 与设备有关,强度数组(长度360)
|
我们主要需要的就是其中的angle_increment
,ranges
。
下面的程序用于将激光雷达数据可视化,更便于理解使用:
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
| import rospy import cv2 import numpy as np import math from sensor_msgs.msg import LaserScan
def callback(data): frame = np.zeros((600, 600,3), np.uint8) angle = data.angle_min cv2.circle(frame, (300,300), 35, (0,255,0), -1) print(len(data.ranges)) for r in data.ranges: if math.isinf(r) == True: r = 1000 x = math.trunc((r * -250.0)*math.cos(angle + (-90.0*3.1416/180.0))) y = math.trunc((r * 250.0)*math.sin(angle + (-90.0*3.1416/180.0))) if y > -300 and y < 300 and x<300 and x>-300: cv2.line(frame,(x+300,y+300),(x+300,y+300),(0,0,255),2) angle= angle + data.angle_increment cv2.imshow('frame',frame) cv2.waitKey(1)
def laser_listener(): rospy.init_node('laser_listener', anonymous=True) rospy.Subscriber("/scan", LaserScan,callback,queue_size = 1) rospy.spin()
if __name__ == '__main__': laser_listener()
|
可以看到显示图像: