一、激光雷达获取数据

激光雷达是地面移动机器人常用的一种传感器，其工作原理如图所示，即用一个高速旋转的激光测距探头，将周围360°的障碍物分布状况测量出来。

激光雷达的旋转部分搭载激光雷达，其在旋转的过程中每隔一定角度就会测量一次距离值。当其旋转一圈时，刚好可以得到一幅周围障碍物轮廓的俯视二维点阵图。激光雷达二维点阵图中的点阵就是激光雷达的输出数值。

1 在仿真环境中实现获取激光雷达数据

编写节点代码：首先需要创建一个ROS源码包。在Ubuntu里打开一个终端程序，输入如下指令进入ROS工作空间

cd catkin_ws/src/

然后输入如下指令创建ROS源码包。
catkin_create_pkg lidar_pkg rospy std_msgs sensor_msgs
按下“Enter”键后，系统会提示ROS源码包创建成功，这时我们可以看到“catkin_ws/src”目录下出现了“lidar_pkg”子目录。

在“scripts”文件夹中（新建文件夹），选择“新建文件”。将这个Python节点文件命名为“lidar_data_node.py”

代码如下：

#!/usr/bin/env python3
# coding=utf-8
import rospy
from sensor_msgs.msg import LaserScan
# 激光雷达回调函数
def cbScan(msg):
rospy.loginfo("雷达数据个数 = %d",len(msg.ranges))
rospy.logwarn("正前方测距数值 = %.2f",msg.ranges[180])
# 主函数
if __name__ == "__main__":
rospy.init_node("lidar_data")

订阅激光雷达的数据话题

lidar_sub = rospy.Subscriber("scan",LaserScan,cbScan,queue_size=10)
rospy.spin()

在按下“Ctrl+S”键进行保存后，关闭。

2．添加可执行权限

由于这个代码文件是新创建的，其默认不带有可执行属性，所以需要为其添加一个可执行属性才能让它运行起来。启动一个终端程序，输入如下指令进入这个代码文件所存放的目录。

cd ~/catkin_ws/src/lidar_pkg/scripts/

再执行如下指令为代码文件添加可执行属性

chmod +x lidar_data_node.py

3．编译软件包
现在节点文件可以运行了，但是这个软件包还没有加入ROS的包管理系统，无法通过ROS指令运行其中的节点，所以还需要对这个软件包进行编译。在终端程序中输入如下指令进入ROS工作空间

cd ~/catkin_ws/

再输入如下指令对软件包进行编译。

catkin_make
编译完成，这时就可以测试此节点了。

4．启动仿真环境
启动开源项目“wpr_simulation”中的仿真场景，打开终端程序，输入如下指令

roslaunch wpr_simulation wpb_simple.launch
输入指令并按“Enter”键后会弹出仿真场景，机器人位于柜子前方。

5．启动图形界面
接下来启动Rviz，如图5-15所示，打开一个新的终端程序，输入如下指令。
roslaunch wpr_simulation wpb_rviz.launch
启动Rviz后会弹出Rviz界面

6．运行节点程序
启动雷达数据获取节点如图5-17所示，再打开一个新的终端程序，输入如下指令。
rosrun lidar_pkg lidar_data_node.py
这条指令会启动前面编写的lidar_data_node.py。按照程序逻辑，系统会从激光雷达的“/scan”主题里不断获取激光雷达数据包，并把测距数值显示在终端程序里。其中接收到的每一帧激光雷达数据的个数都是 360。正前方的测距数值是一个浮点数，单位是米。比如终端里显示“正前方距离数值 = 1.75”，表示激光雷达在180°的这个角度也就是机器人的正前方测量到的其与障碍物的距离值是1.75米，以此类推。

二、利用激光雷达实现简单避障

结合机器人运动终止的方法和激光雷达的数据获取方法，实现一个根据激光雷达测距信息进行避障运动的闭环行为。

激光雷达旋转一周的扫描角度范围为0°到360°，机器人正前方的激光射线角度为扫描角度范围的中间值，也就是360°的一半，为180°。在程序实现的时候，我们只需要将180°方向上的激光雷达测距数值作为判断依据，控制机器人旋转和直行即可。

1．编写节点代码
首先需要创建一个ROS源码包。在Ubuntu里打开一个终端程序，输入如下指令进入ROS工作空间

cd catkin_ws/src/

然后输入如下指令创建ROS源码包。
catkin_create_pkg behavior_pkg rospy std_msgs sensor_msgs geometry_msgs
按下“Enter”键后即创建behavior_pkg源码包

同前建立“behavior_node.py”文件

编写如下代码：

#!/usr/bin/env python3
# coding=utf-8
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
count = 0
# 激光雷达回调函数
def cbScan(msg):
global vel_pub
global count
vel_msg = Twist()
dist = msg.ranges[180]
rospy.logwarn("正前方测距数值 = %.2f",dist)

if count > 0:
count = count -1
rospy.loginfo("持续转向 count = %d",count)
return
if dist > 1.5:
vel_msg.linear.x = 0.05
else:
vel_msg.angular.z = 0.3
count = 50
vel_pub.publish(vel_msg)
# 主函数
if __name__ == "__main__":
rospy.init_node("behavior_node")

发布机器人运动控制话题

vel_pub = rospy.Publisher("cmd_vel",Twist,queue_size=10)

订阅激光雷达的数据话题

lidar_sub = rospy.Subscriber("scan",LaserScan,cbScan,queue_size=10)
rospy.spin()

2．设置可执行权限
由于这个代码文件是新创建的，其默认不带有可执行属性，所以我们需要为其添加一个可执行属性让它能够运行起来。启动一个终端程序，输入如下指令进入这个代码文件所存放的目录。
cd ~/catkin_ws/src/behavior_pkg/scripts/
再输入如下指令为代码文件添加可执行属性。
chmod +x behavior_node.py
设置文件权限，

3．编译软件包
现在节点文件可以运行了，但是这个软件包还没有加入ROS的包管理系统，无法通过ROS指令运行其中的节点，所以还需要对这个软件包进行编译。在终端程序中输入如下指令进入ROS工作空间。
cd ~/catkin_ws/
再输入如下指令对软件包进行编译。
catkin_make
编译完成就可以测试此节点了。

4．启动仿真环境
启动开源项目“wpr_simulation”中的仿真场景，打开终端程序，输入如下指令。
roslaunch wpr_simulation wpb_simple.launch
启动后会弹出仿真场景，机器人位于柜子前方

5．启动图形界面
启动Rviz，打开一个新的终端程序，输入如下指令。
roslaunch wpr_simulation wpb_rviz.launch
执行指令后系统会弹出Rviz界面。

6．运行节点程序
启动简单避障节点，再打开一个新的终端程序，输入如下指令。
rosrun behavior_pkg behavior_node.py

这条指令会启动上面所编写的behavior_node.py。按照程序逻辑，会从激光雷达的“/scan”主题里不断获取激光雷达数据包，并把机器人正前方的激光雷达测距数值显示在终端程序里。终端里显示“正前方测距数值= xxx”，其中xxx为一个浮点数，单位是“米”，比如“正前方测距数值 = 2.61”表示机器人正前方的激光雷达测距值为2.61米

程序启动后，机器人开始以0.05米/秒的速度向前移动。当机器人前方1.5米处出现障碍物时，机器人停止移动，以0.3弧度/秒的速度原地转动。当机器人转到一定角度后，停止转动，继续以0.05米/秒的速度向前移动。在仿真界面中可以看到机器人在柜子前转向绕开柜子。

来源: 部分来自网络