一、激光雷達(dá)獲取數(shù)據(jù)

激光雷達(dá)是地面移動機(jī)器人常用的一種傳感器，其工作原理如圖所示，即用一個高速旋轉(zhuǎn)的激光測距探頭，將周圍360°的障礙物分布狀況測量出來。

激光雷達(dá)的旋轉(zhuǎn)部分搭載激光雷達(dá)，其在旋轉(zhuǎn)的過程中每隔一定角度就會測量一次距離值。當(dāng)其旋轉(zhuǎn)一圈時，剛好可以得到一幅周圍障礙物輪廓的俯視二維點(diǎn)陣圖。激光雷達(dá)二維點(diǎn)陣圖中的點(diǎn)陣就是激光雷達(dá)的輸出數(shù)值。

1 在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)

編寫節(jié)點(diǎn)代碼：首先需要創(chuàng)建一個ROS源碼包。在Ubuntu里打開一個終端程序，輸入如下指令進(jìn)入ROS工作空間

cd catkin_ws/src/

然后輸入如下指令創(chuàng)建ROS源碼包。
catkin_create_pkg lidar_pkg rospy std_msgs sensor_msgs
按下“Enter”鍵后，系統(tǒng)會提示ROS源碼包創(chuàng)建成功，這時我們可以看到“catkin_ws/src”目錄下出現(xiàn)了“l(fā)idar_pkg”子目錄。

在“scripts”文件夾中（新建文件夾），選擇“新建文件”。將這個Python節(jié)點(diǎn)文件命名為“l(fā)idar_data_node.py”

代碼如下：

#!/usr/bin/env python3
# coding=utf-8
import rospy
from sensor_msgs.msg import LaserScan
# 激光雷達(dá)回調(diào)函數(shù)
def cbScan(msg):
rospy.loginfo("雷達(dá)數(shù)據(jù)個數(shù) = %d",len(msg.ranges))
rospy.logwarn("正前方測距數(shù)值 = %.2f",msg.ranges[180])
# 主函數(shù)
if __name__ == "__main__":
rospy.init_node("lidar_data")

訂閱激光雷達(dá)的數(shù)據(jù)話題

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

在按下“Ctrl+S”鍵進(jìn)行保存后，關(guān)閉。

2．添加可執(zhí)行權(quán)限

由于這個代碼文件是新創(chuàng)建的，其默認(rèn)不帶有可執(zhí)行屬性，所以需要為其添加一個可執(zhí)行屬性才能讓它運(yùn)行起來。啟動一個終端程序，輸入如下指令進(jìn)入這個代碼文件所存放的目錄。

cd ~/catkin_ws/src/lidar_pkg/scripts/

再執(zhí)行如下指令為代碼文件添加可執(zhí)行屬性

chmod +x lidar_data_node.py

3．編譯軟件包
現(xiàn)在節(jié)點(diǎn)文件可以運(yùn)行了，但是這個軟件包還沒有加入ROS的包管理系統(tǒng)，無法通過ROS指令運(yùn)行其中的節(jié)點(diǎn)，所以還需要對這個軟件包進(jìn)行編譯。在終端程序中輸入如下指令進(jìn)入ROS工作空間

cd ~/catkin_ws/

再輸入如下指令對軟件包進(jìn)行編譯。

catkin_make
編譯完成，這時就可以測試此節(jié)點(diǎn)了。

4．啟動仿真環(huán)境
啟動開源項(xiàng)目“wpr_simulation”中的仿真場景，打開終端程序，輸入如下指令

roslaunch wpr_simulation wpb_simple.launch
輸入指令并按“Enter”鍵后會彈出仿真場景，機(jī)器人位于柜子前方。

5．啟動圖形界面
接下來啟動Rviz，如圖5-15所示，打開一個新的終端程序，輸入如下指令。
roslaunch wpr_simulation wpb_rviz.launch
啟動Rviz后會彈出Rviz界面

6．運(yùn)行節(jié)點(diǎn)程序
啟動雷達(dá)數(shù)據(jù)獲取節(jié)點(diǎn)如圖5-17所示，再打開一個新的終端程序，輸入如下指令。
rosrun lidar_pkg lidar_data_node.py
這條指令會啟動前面編寫的lidar_data_node.py。按照程序邏輯，系統(tǒng)會從激光雷達(dá)的“/scan”主題里不斷獲取激光雷達(dá)數(shù)據(jù)包，并把測距數(shù)值顯示在終端程序里。其中接收到的每一幀激光雷達(dá)數(shù)據(jù)的個數(shù)都是 360。正前方的測距數(shù)值是一個浮點(diǎn)數(shù)，單位是米。比如終端里顯示“正前方距離數(shù)值 = 1.75”，表示激光雷達(dá)在180°的這個角度也就是機(jī)器人的正前方測量到的其與障礙物的距離值是1.75米，以此類推。

二、利用激光雷達(dá)實(shí)現(xiàn)簡單避障

結(jié)合機(jī)器人運(yùn)動終止的方法和激光雷達(dá)的數(shù)據(jù)獲取方法，實(shí)現(xiàn)一個根據(jù)激光雷達(dá)測距信息進(jìn)行避障運(yùn)動的閉環(huán)行為。

激光雷達(dá)旋轉(zhuǎn)一周的掃描角度范圍為0°到360°，機(jī)器人正前方的激光射線角度為掃描角度范圍的中間值，也就是360°的一半，為180°。在程序?qū)崿F(xiàn)的時候，我們只需要將180°方向上的激光雷達(dá)測距數(shù)值作為判斷依據(jù)，控制機(jī)器人旋轉(zhuǎn)和直行即可。

1．編寫節(jié)點(diǎn)代碼
首先需要創(chuàng)建一個ROS源碼包。在Ubuntu里打開一個終端程序，輸入如下指令進(jìn)入ROS工作空間

cd catkin_ws/src/

然后輸入如下指令創(chuàng)建ROS源碼包。
catkin_create_pkg behavior_pkg rospy std_msgs sensor_msgs geometry_msgs
按下“Enter”鍵后即創(chuàng)建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
# 激光雷達(dá)回調(diào)函數(shù)
def cbScan(msg):
global vel_pub
global count
vel_msg = Twist()
dist = msg.ranges[180]
rospy.logwarn("正前方測距數(shù)值 = %.2f",dist)

if count > 0:
count = count -1
rospy.loginfo("持續(xù)轉(zhuǎn)向 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)
# 主函數(shù)
if __name__ == "__main__":
rospy.init_node("behavior_node")

發(fā)布機(jī)器人運(yùn)動控制話題

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

訂閱激光雷達(dá)的數(shù)據(jù)話題

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

2．設(shè)置可執(zhí)行權(quán)限
由于這個代碼文件是新創(chuàng)建的，其默認(rèn)不帶有可執(zhí)行屬性，所以我們需要為其添加一個可執(zhí)行屬性讓它能夠運(yùn)行起來。啟動一個終端程序，輸入如下指令進(jìn)入這個代碼文件所存放的目錄。
cd ~/catkin_ws/src/behavior_pkg/scripts/
再輸入如下指令為代碼文件添加可執(zhí)行屬性。
chmod +x behavior_node.py
設(shè)置文件權(quán)限，

3．編譯軟件包
現(xiàn)在節(jié)點(diǎn)文件可以運(yùn)行了，但是這個軟件包還沒有加入ROS的包管理系統(tǒng)，無法通過ROS指令運(yùn)行其中的節(jié)點(diǎn)，所以還需要對這個軟件包進(jìn)行編譯。在終端程序中輸入如下指令進(jìn)入ROS工作空間。
cd ~/catkin_ws/
再輸入如下指令對軟件包進(jìn)行編譯。
catkin_make
編譯完成就可以測試此節(jié)點(diǎn)了。

4．啟動仿真環(huán)境
啟動開源項(xiàng)目“wpr_simulation”中的仿真場景，打開終端程序，輸入如下指令。
roslaunch wpr_simulation wpb_simple.launch
啟動后會彈出仿真場景，機(jī)器人位于柜子前方

5．啟動圖形界面
啟動Rviz，打開一個新的終端程序，輸入如下指令。
roslaunch wpr_simulation wpb_rviz.launch
執(zhí)行指令后系統(tǒng)會彈出Rviz界面。

6．運(yùn)行節(jié)點(diǎn)程序
啟動簡單避障節(jié)點(diǎn)，再打開一個新的終端程序，輸入如下指令。
rosrun behavior_pkg behavior_node.py

這條指令會啟動上面所編寫的behavior_node.py。按照程序邏輯，會從激光雷達(dá)的“/scan”主題里不斷獲取激光雷達(dá)數(shù)據(jù)包，并把機(jī)器人正前方的激光雷達(dá)測距數(shù)值顯示在終端程序里。終端里顯示“正前方測距數(shù)值= xxx”，其中xxx為一個浮點(diǎn)數(shù)，單位是“米”，比如“正前方測距數(shù)值 = 2.61”表示機(jī)器人正前方的激光雷達(dá)測距值為2.61米

程序啟動后，機(jī)器人開始以0.05米/秒的速度向前移動。當(dāng)機(jī)器人前方1.5米處出現(xiàn)障礙物時，機(jī)器人停止移動，以0.3弧度/秒的速度原地轉(zhuǎn)動。當(dāng)機(jī)器人轉(zhuǎn)到一定角度后，停止轉(zhuǎn)動，繼續(xù)以0.05米/秒的速度向前移動。在仿真界面中可以看到機(jī)器人在柜子前轉(zhuǎn)向繞開柜子。

來源: 部分來自網(wǎng)絡(luò)