雷达模块
单线雷达:
一线激光雷达测距系统是通过 uart 串口信号与外部系统通讯, 依据通讯协议,外部 系统调用 SDK 函数命令主要是对 Lidar 的扫描数据进行实时获取,设置设备工作状态以及工作模式。
数据通信接口函数
SDk 主要提供了该雷达的驱动类 Lidar_driver,其中主要的函数如下:
(a)初始化串口函数 ::OpenLidarSerial();
该函数完成串口参数的初始化,打开设备接受线程。
(b)关闭串口函数 ::CloseLidarSerial();
该函数主要完成串口设备的线程释放。
(c)命令函数 ::SendLidarCommand();
该函数完成对雷达的数据指令,参数包括:
START:打开设备电源,电机旋转。
SCAN:打开数据模块开关,发送单圈数据 DATA_SCAN: 打开数据模块开关,连续发送雷 达数据 DATA_STOP: 关闭数据开关,停止发送数据。
STOP:关闭设备电源,电机停止转动
SWITCH_ANGLE: 角度信息切换
SWITCH_STRENGHT: 强度信息切换
CHSPEED: 修改电机转速
CHRESOLUTION:修改角度分辨率
(d)获取数据函数 ::GetLidarScanData();
实例程序
向 Linux 系统中拷贝 LS01BLinux 驱动
使用 LSlidarUSB 转串口说明
使用附赠 USB 数据线将一线激光雷达与计算机连接,在 Ubuntu 系统终端中输入一下 命令进入查看 USB 转串口节点命令,命令行及效果如图
如 果 USB 转串口连接成功,会看到一个 ttyUSB0 的节点(本人 ubuntu15.10 中测试, 不需要安装 cp2102 驱动)。在发现 ttyUSB0 后在终端中输入一下命令给 USB 转串口权限
使用 LSlidar 界面程序说明
在驱动文件夹下,打开 linux 终端,获得串口的权限后直接输入命令./ls01b,运行界面
在 ubuntu 系统中安装 Q(t 本人使用的是 Qt5.5.0);用 QtCreator 打开《LSlidarlinux 源程序及驱动 qt》程序 230400.pro,界面如图
点 击左下角的绿色三角形运行程序,如果激光雷达有连接,然后点击打开按钮,会有下 面的界面和数据
点 击左上角显示按钮会出
16线激光雷达
16 线激光雷达主要面向无人驾驶汽车环境感知、机器人环境感知、无人机测绘等领域, 通过 16 个激光发射组件快速旋转的同时发射高频率激光束对外界环境进行持续性的扫描, 经过测距算法提供三维空间数据及物体反射率,可以让机器看到周围的世界。
建立 ROS 工作空间
$mkdir -p ~/ros_ws/src
线激光雷达解析程序编译
$cd ~/ros_ws
$catkin_make
$source devel/setup.bash
$roslaunch lslidar_c16_decoder lslidar_c16.launch
$source ~/ros_ws/devel/setup.bash
$echo “source ~/ros_ws/devel/setup.bash” >> 〜/.bashrc
$source ~/.bashrc
$rviz
在弹出的 Displays 窗口中,如图 49 所示,将“FixedFrame”的值修改成 laser_link 即可,同时点击 add 按钮,在 Bytopic 下点击 PointCloud2 添加多线点云节点,即可得到 16 线雷达数据可视化界面
中心点计算
$roscore
$rosbag play floor.bag -l
$rviz
创建名为 center_line.py 的 python 文件,并输入下列代码
#!/usr/bin/python
# -*- coding: utf-8 -*
#导入必要的库 import sys
import os.path
import numpy as np
import rospy
import random from sensor_msgs.msg
import PointCloud2, PointField
import sensor_msgs.point_cloud2 as pc2 from std_msgs.msg
import Header
class CenterLine(): #构造函数
def __init__(self): self.pcl_sub = rospy.Subscriber("lslidar_point_cloud", PointCloud2, self.prediction_publis h)
self.pcl_pub = rospy.Publisher("pcl_left", PointCloud2, queue_size=10)
self.pcl_pub2 = rospy.Publisher("pcl_right", PointCloud2, queue_size=10)
self.center_pub = rospy.Publisher("center_points", PointCloud2, queue_size=1) rospy.spin()
#创建点云函数
def create_cloud_xyz32(self, header, points):
fields = [PointField('x', 0, PointField.FLOAT32, 1),
PointField('y', 4, PointField.FLOAT32, 1),
PointField('z', 8, PointField.FLOAT32, 1)]
return pc2.create_cloud(header, fields, points)
#回调函数,用于处理订阅的话题数据,并发布自己的数据
def prediction_publish(self, lidar):
lidar = pc2.read_points(lidar)
points = np.array(list(lidar))
points = points[np.where([-0.08 <= point[2] <= 0.08 and point[0] > 0 for point in poi nts])] np.unique(points)
l_points = points[np.where([point[1] > 0 for point in points])]
r_points = points[np.where([point[1] < 0 for point in points])]
l_points = np.array(random.sample(l_points,min(len(l_points),len(r_points))))
r_points = np.array(random.sample(r_points,min(len(l_points),len(r_points))))
l_points = l_points[np.argsort(l_points.T)[0]]
r_points = r_points[np.argsort(r_points.T)[0]]
min_points = min(len(l_points),len(r_points))
center_x = np.array([(l_points[i,0]+r_points[i,0])/2 for i in range(min_points)]).reshape(
1)
center_y = np.array([(l_points[j,1]+r_points[j,1])/2 for j in range(min_points)]).reshape(
1)
center_z = np.zeros(len(center_x)).reshape(-1)
center_cloud = np.stack((center_x, center_y, center_z))
center_points = [] for x,y in zip(list(center_x),list(center_y)):
center_point = x,y center_points.append(center_point)
f = open('./1.txt','w')
f.write(str(center_points))
f.close()
header = Header()
header.stamp = rospy.Time().now()
header.frame_id = "laser_link"
cloud_filtered = self.create_cloud_xyz32(header, center_cloud.T) self.center_pub.publish(cloud_filtered)
l_x = l_points[:, 0].reshape(-1)
l_y = l_points[:, 1].reshape(-1)
l_z = np.zeros(len(l_points)).reshape(-1)
l_cloud = np.stack((l_x, l_y, l_z))
r_x = r_points[:, 0].reshape(-1)
r_y = r_points[:, 1].reshape(-1)
r_z = np.zeros(len(r_points)).reshape(-1)
r_cloud = np.stack((r_x, r_y, r_z))
l_header = Header()
l_header.stamp = rospy.Time().now()
l_header.frame_id = "laser_link"
l_cloud_filtered = self.create_cloud_xyz32(l_header, l_cloud.T) self.pcl_pub.publish(l_cloud_filtered)
r_header = Header()
r_header.stamp = rospy.Time().now()
r_header.frame_id = "laser_link"
r_cloud_filtered = self.create_cloud_xyz32(r_header, r_cloud.T) self.pcl_pub2.publish(r_cloud_filtered)
if __name__ == '__main__':
#初始化一个节点
rospy.init_node('center_line_node')
#类实例化
center_line = CenterLine()
运行 center_line.py,并使用 rviz 可视化结果如下图所示