輪式智能移動(dòng)操作機(jī)器人技術(shù)與應(yīng)用-基于ROS的Python編程 課件 第5章 激光雷達(dá)應(yīng)用實(shí)例

第5章激光雷達(dá)應(yīng)用實(shí)例目錄5.1獲取激光雷達(dá)數(shù)據(jù)5.2利用激光雷達(dá)實(shí)現(xiàn)簡單避障5.3本章小結(jié)激光雷達(dá)是地面移動(dòng)機(jī)器人常用的一種傳感器，其工作原理如圖5-1所示，即用一個(gè)高速旋轉(zhuǎn)的激光測距探頭，將周圍360°的障礙物分布狀況測量出來。圖5-1激光雷達(dá)工作原理激光雷達(dá)的旋轉(zhuǎn)部分搭載激光雷達(dá)，其在旋轉(zhuǎn)的過程中每隔一定角度就會(huì)測量一次距離值。當(dāng)其旋轉(zhuǎn)一圈時(shí)，剛好可以得到一幅周圍障礙物輪廓的俯視二維點(diǎn)陣圖。激光雷達(dá)二維點(diǎn)陣圖如圖5-2所示，這個(gè)二維點(diǎn)陣圖中的點(diǎn)陣就是激光雷達(dá)的輸出數(shù)值。圖5-2激光雷達(dá)二維點(diǎn)陣圖5.1獲取激光雷達(dá)數(shù)據(jù)5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)1．編寫節(jié)點(diǎn)代碼首先需要?jiǎng)?chuàng)建一個(gè)ROS源碼包。在Ubuntu里打開一個(gè)終端程序，輸入如下指令進(jìn)入ROS工作空間（見圖5-3）。cdcatkin_ws/src/圖5-3進(jìn)入ROS工作空間5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)然后輸入如下指令創(chuàng)建ROS源碼包。catkin_create_pkglidar_pkgrospystd_msgssensor_msgs按下“Enter”鍵后，系統(tǒng)會(huì)提示ROS源碼包創(chuàng)建成功（見圖5-4），這時(shí)我們可以看到“catkin_ws/src”目錄下出現(xiàn)了“l(fā)idar_pkg”子目錄。圖5-4創(chuàng)建lidar_pkg源碼包5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)創(chuàng)建lidar_pkg源碼包的指令含義，如表5-1所示。表5-1創(chuàng)建lidar_pkg源碼包的指令含義指令含義catkin_create_pkg創(chuàng)建ROS源碼包（Package）的指令lidar_pkg新建的ROS源碼包命名rospyPython依賴項(xiàng)，因?yàn)楸纠淌褂肞ython編寫，所以需要這個(gè)依賴項(xiàng)std_msgs標(biāo)準(zhǔn)消息依賴項(xiàng)，需要里面的String格式做文字輸出sensor_msgs傳感器消息依賴項(xiàng)，激光雷達(dá)數(shù)據(jù)格式需要此項(xiàng)5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)接下來在VisualStudioCode中進(jìn)行操作，將目錄展開，找到前面新建的“l(fā)idar_pkg”并展開，可以看到它是一個(gè)功能包最基礎(chǔ)的結(jié)構(gòu)，選中“l(fā)idar_pkg”并對其右擊，彈出圖5-5所示的快捷菜單，選擇“新建文件夾”。圖5-5快捷菜單5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)將這個(gè)文件夾命名為“scripts”，如圖5-6所示。在第2章的2.3節(jié)中，有對此名稱文件夾的解釋。命名成功后按“Enter”鍵則創(chuàng)建成功。圖5-6創(chuàng)建“scripts”文件夾選中此文件夾并對其右擊，彈出圖5-5所示的快捷菜單，選擇“新建文件”。將這個(gè)Python節(jié)點(diǎn)文件命名為“l(fā)idar_data_node.py”。5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)命名完成后即可在IDE右側(cè)開始編寫“l(fā)idar_data_node.py”的代碼，其內(nèi)容如下。#!/usr/bin/envpython3#coding=utf-8importrospyfromsensor_msgs.msgimportLaserScan#激光雷達(dá)回調(diào)函數(shù)defcbScan(msg):rospy.loginfo("雷達(dá)數(shù)據(jù)個(gè)數(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()5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)（1）代碼的開始部分，使用Shebang符號指定這個(gè)Python文件的解釋器為python3。如果是Ubuntu18.04或更早的版本，解釋器可設(shè)為python。（2）第二句代碼指定該文件的字符編碼為utf-8，這樣就能在代碼執(zhí)行的時(shí)候顯示中文字符。（3）使用import導(dǎo)入兩個(gè)模塊，一個(gè)是rospy模塊，包含了大部分的ROS接口函數(shù)；另一個(gè)是激光雷達(dá)數(shù)據(jù)的消息類型，即sensor_msgs.msg里的LaserScan。（4）定義一個(gè)回調(diào)函數(shù)cbScan()，用來處理激光雷達(dá)數(shù)據(jù)。ROS每接收到一幀激光雷達(dá)數(shù)據(jù)，就會(huì)自動(dòng)調(diào)用一次回調(diào)函數(shù)。激光雷達(dá)的測距數(shù)值會(huì)以參數(shù)的形式傳遞到這個(gè)回調(diào)函數(shù)里。（5）在回調(diào)函數(shù)cbScan()中，從參數(shù)msg里獲取激光雷達(dá)數(shù)據(jù)，先調(diào)用rospy的loginfo()函數(shù)顯示激光雷達(dá)測距數(shù)據(jù)的個(gè)數(shù)（通常是360個(gè)），然后調(diào)用rospy的logwarn()函數(shù)顯示下標(biāo)為180的測距數(shù)值，也就是最中間那個(gè)測距值，對應(yīng)的是機(jī)器人正前方的測距值。參數(shù)msg是一個(gè)sensor_msgs::LaserScan類型的數(shù)據(jù)包，其數(shù)據(jù)格式定義如圖5-7所示，其中float32[]ranges數(shù)組存放的是激光雷達(dá)的測距數(shù)值。這里機(jī)器人使用的是RPLidarA2型激光雷達(dá)，其旋轉(zhuǎn)一周可測量360個(gè)距離值，所以在代碼里，ranges是一個(gè)有360個(gè)成員的距離數(shù)組。5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)（6）判斷__name__為“__main__”時(shí)，執(zhí)行這個(gè)文件的主函數(shù)代碼。（7）調(diào)用rospy的init_node()函數(shù)進(jìn)行該節(jié)點(diǎn)的初始化操作，參數(shù)是節(jié)點(diǎn)名稱。（8）調(diào)用rospy的Subscriber()函數(shù)生成一個(gè)訂閱對象lidar_sub，在函數(shù)的參數(shù)中指明這個(gè)對象訂閱的是“scan”話題，數(shù)據(jù)類型為LaserScan，回調(diào)函數(shù)設(shè)置為之前定義的回調(diào)函數(shù)，緩沖長度為10。（9）調(diào)用rospy的spin()對這個(gè)主函數(shù)進(jìn)行阻塞，保持這個(gè)節(jié)點(diǎn)程序不會(huì)結(jié)束退出。5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)圖5-7sensor_msgs::LaserScan類型的格式5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)程序編寫完后，代碼并未馬上保存到文件里，此時(shí)界面左上編輯區(qū)的文件名“l(fā)idar_data_node.py”右側(cè)有個(gè)白色小圓點(diǎn)（見圖5-8），這表示此文件并未保存。在按下“Ctrl+S”鍵進(jìn)行保存后，白色小圓點(diǎn)會(huì)變成關(guān)閉按鈕“×”。圖5-8文件未保存狀態(tài)5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)2．添加可執(zhí)行權(quán)限由于這個(gè)代碼文件是新創(chuàng)建的，其默認(rèn)不帶有可執(zhí)行屬性，所以需要為其添加一個(gè)可執(zhí)行屬性才能讓它運(yùn)行起來。啟動(dòng)一個(gè)終端程序，輸入如下指令進(jìn)入這個(gè)代碼文件所存放的目錄（見圖5-9）。cd~/catkin_ws/src/lidar_pkg/scripts/圖5-9進(jìn)入目錄5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)再執(zhí)行如下指令為代碼文件添加可執(zhí)行屬性。圖5-10設(shè)置文件權(quán)限chmod+xlidar_data_node.py設(shè)置文件權(quán)限如圖5-10所示，按“Enter”鍵執(zhí)行后，這個(gè)代碼文件就獲得了可執(zhí)行屬性，可以在終端程序里運(yùn)行了。5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)圖5-11進(jìn)入ROS工作空間3．編譯軟件包現(xiàn)在節(jié)點(diǎn)文件可以運(yùn)行了，但是這個(gè)軟件包還沒有加入ROS的包管理系統(tǒng)，無法通過ROS指令運(yùn)行其中的節(jié)點(diǎn)，所以還需要對這個(gè)軟件包進(jìn)行編譯。在終端程序中輸入如下指令進(jìn)入ROS工作空間（見圖5-11）。cd~/catkin_ws/5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)再輸入如下指令對軟件包進(jìn)行編譯。catkin_make編譯完成如圖5-12所示，這時(shí)就可以測試此節(jié)點(diǎn)了。圖5-12編譯完成5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)4．啟動(dòng)仿真環(huán)境啟動(dòng)開源項(xiàng)目“wpr_simulation”中的仿真場景，打開終端程序，輸入如下指令，如圖5-13所示。roslaunchwpr_simulationwpb_simple.launch輸入指令并按“Enter”鍵后會(huì)彈出圖5-14所示的仿真場景，機(jī)器人位于柜子前方。圖5-13啟動(dòng)仿真場景圖5-14仿真場景5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)5．啟動(dòng)圖形界面接下來啟動(dòng)Rviz，如圖5-15所示，打開一個(gè)新的終端程序，輸入如下指令。roslaunchwpr_simulationwpb_rviz.launch啟動(dòng)Rviz后會(huì)彈出圖5-16所示的Rviz界面。圖5-15啟動(dòng)Rviz圖5-16Rviz界面5.1.1在仿真環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)6．運(yùn)行節(jié)點(diǎn)程序啟動(dòng)雷達(dá)數(shù)據(jù)獲取節(jié)點(diǎn)如圖5-17所示，再打開一個(gè)新的終端程序，輸入如下指令。rosrunlidar_pkglidar_data_node.py這條指令會(huì)啟動(dòng)前面編寫的lidar_data_node.py。按照程序邏輯，系統(tǒng)會(huì)從激光雷達(dá)的“/scan”主題里不斷獲取激光雷達(dá)數(shù)據(jù)包，并把測距數(shù)值顯示在終端程序里。其中接收到的每一幀激光雷達(dá)數(shù)據(jù)的個(gè)數(shù)都是360。正前方的測距數(shù)值是一個(gè)浮點(diǎn)數(shù)，單位是米。比如終端里顯示“正前方距離數(shù)值=1.75”，表示激光雷達(dá)在180°的這個(gè)角度也就是機(jī)器人的正前方測量到的其與障礙物的距離值是1.75米，以此類推。圖5-17啟動(dòng)雷達(dá)數(shù)據(jù)獲取節(jié)點(diǎn)5.1.2在真實(shí)環(huán)境中實(shí)現(xiàn)獲取激光雷達(dá)數(shù)據(jù)這里的程序可以在啟智ROS機(jī)器人上運(yùn)行，具體步驟如下。（1）根據(jù)啟智ROS的實(shí)驗(yàn)指導(dǎo)書對運(yùn)行環(huán)境和驅(qū)動(dòng)源碼包進(jìn)行配置。（2）把本章所創(chuàng)建的源碼包“l(fā)idar_pkg”復(fù)制到機(jī)載計(jì)算機(jī)的“~/catkin_ws/src”目錄下進(jìn)行編譯。（3）連接好設(shè)備上的硬件。（4）使用機(jī)載計(jì)算機(jī)打開終端程序，輸入如下指令。roslaunchwpb_home_bringuplidar_base.launch（5）運(yùn)行“l(fā)idar_data_node”節(jié)點(diǎn)，打開一個(gè)新的終端程序，運(yùn)行如下指令。rosrunlidar_pkglidar_data_node.py按“Enter”鍵運(yùn)行后，即可利用“l(fā)idar_data_node”節(jié)點(diǎn)獲取啟智ROS機(jī)器人的雷達(dá)數(shù)據(jù)信息。5.2利用激光雷達(dá)實(shí)現(xiàn)簡單避障前面介紹了機(jī)器人運(yùn)動(dòng)終止的方法，也介紹了激光雷達(dá)的數(shù)據(jù)獲取方法，本節(jié)將把它們結(jié)合起來，實(shí)現(xiàn)一個(gè)根據(jù)激光雷達(dá)測距信息進(jìn)行避障運(yùn)動(dòng)的閉環(huán)行為。在開始之前，我們需要先了解一下在本節(jié)所使用的開源仿真項(xiàng)目中，激光雷達(dá)是如何安裝在機(jī)器人上的（見圖5-18），激光雷達(dá)旋轉(zhuǎn)一周的掃描角度范圍為0°到360°，機(jī)器人正前方的激光射線角度為掃描角度范圍的中間值，也就是360°的一半，為180°。在程序?qū)崿F(xiàn)的時(shí)候，我們只需要將180°方向上的激光雷達(dá)測距數(shù)值作為判斷依據(jù)，控制機(jī)器人旋轉(zhuǎn)和直行即可。圖5-18激光雷達(dá)在機(jī)器人上的安裝方式5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障1．編寫節(jié)點(diǎn)代碼首先需要?jiǎng)?chuàng)建一個(gè)ROS源碼包。在Ubuntu里打開一個(gè)終端程序，輸入如下指令進(jìn)入ROS工作空間，如圖5-19所示。cdcatkin_ws/src/圖5-19進(jìn)入ROS工作空間5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障然后輸入如下指令創(chuàng)建ROS源碼包。catkin_create_pkgbehavior_pkgrospystd_msgssensor_msgsgeometry_msgs按下“Enter”鍵后即創(chuàng)建behavior_pkg源碼包，如圖5-20所示，系統(tǒng)會(huì)提示ROS源碼包創(chuàng)建成功，這時(shí)我們可以看到“catkin_ws/src”目錄下出現(xiàn)了“behavior_pkg”子目錄。圖5-20創(chuàng)建behavior_pkg源碼包5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障創(chuàng)建behavior_pkg源碼包的指令含義，如表5-2所示。表5-2創(chuàng)建behavior_pkg源碼包的指令含義指令含義catkin_create_pkg創(chuàng)建ROS源碼包（Package）的指令behavior_pkg新建的ROS源碼包命名rospyPython依賴項(xiàng)，因?yàn)楸纠淌褂肞ython編寫，所以需要這個(gè)依賴項(xiàng)std_msgs標(biāo)準(zhǔn)消息依賴項(xiàng)，需要里面的String格式做文字輸出sensor_msgs傳感器消息依賴項(xiàng)，激光雷達(dá)數(shù)據(jù)格式需要此項(xiàng)geometry_msgs速度消息依賴項(xiàng)，需要里面的Twist格式描述運(yùn)動(dòng)速度5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障接下來在VisualStudioCode中進(jìn)行操作，將目錄展開，找到前面新建的“behavior_pkg”并展開，可以看到它是一個(gè)功能包最基礎(chǔ)的結(jié)構(gòu)，選中“behavior_pkg”并對其右擊，彈出圖5-21所示的快捷菜單，選擇“新建文件夾”。圖5-21快捷菜單5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障將這個(gè)文件夾命名為“scripts”，如圖5-22所示。在第2章的2.3節(jié)中，有對此名稱文件夾的解釋。命名成功后按“Enter”鍵則創(chuàng)建成功。圖5-22創(chuàng)建“scripts”文件夾選中此文件夾并對其右擊，彈出圖5-21所示的快捷菜單，選擇“新建文件”。將這個(gè)Python節(jié)點(diǎn)文件命名為“behavior_node.py”。5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障命名完成后即可在IDE右側(cè)開始編寫“behavior_node.py”的代碼，其內(nèi)容如下。#!/usr/bin/envpython3#coding=utf-8importrospyfromsensor_msgs.msgimportLaserScanfromgeometry_msgs.msgimportTwist

count=0#激光雷達(dá)回調(diào)函數(shù)defcbScan(msg):globalvel_pubglobalcountvel_msg=Twist()dist=msg.ranges[180]rospy.logwarn("正前方測距數(shù)值=%.2f",dist)5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障ifcount>0:count=count-1rospy.loginfo("持續(xù)轉(zhuǎn)向count=%d",count)returnifdist>1.5:vel_msg.linear.x=0.05else:vel_msg.angular.z=0.3count=50vel_pub.publish(vel_msg)#主函數(shù)if__name__=="__main__":rospy.init_node("behavior_node")#發(fā)布機(jī)器人運(yùn)動(dòng)控制話題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()5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障

（1）代碼的開始部分，使用Shebang符號指定這個(gè)Python文件的解釋器為python3。如果是Ubuntu18.04或更早的版本，解釋器可設(shè)為python。（2）第二句代碼指定該文件的字符編碼為utf-8，這樣就能在代碼執(zhí)行的時(shí)候顯示中文字符。（3）使用import導(dǎo)入三個(gè)模塊，第一個(gè)是rospy模塊，包含了大部分的ROS接口函數(shù)；第二個(gè)是激光雷達(dá)數(shù)據(jù)的消息類型，即sensor_msgs.msg里的LaserScan；第三個(gè)是運(yùn)動(dòng)速度的消息類型，即geometry_msgs.msg里的Twist，其數(shù)據(jù)格式如圖5-23所示。圖5-23Twist.msg數(shù)據(jù)格式5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障

其中l(wèi)inear是運(yùn)動(dòng)控制的線性分量，也就是機(jī)器人直線移動(dòng)的分量，angular是機(jī)器人旋轉(zhuǎn)運(yùn)動(dòng)的分量。這兩個(gè)分量都是Vector3類型，其數(shù)據(jù)格式如圖5-24所示。圖5-24Vector3.msg數(shù)據(jù)格式由此可見，Vector3類型包含三個(gè)浮點(diǎn)數(shù)：x、y和z。對于linear來說，x、y和z對應(yīng)的是沿X軸、Y軸和Z軸的速度分量，分量數(shù)值單位為“米/秒”。對于angular來說，x、y和z對應(yīng)的是以X軸、Y軸和Z軸為旋轉(zhuǎn)軸的旋轉(zhuǎn)速度分量，分量數(shù)值單位為“弧度/秒”。5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障

（4）接下來定義一個(gè)全局變量count并賦初值為0，后面會(huì)用這個(gè)變量做一個(gè)計(jì)時(shí)器，控制機(jī)器人轉(zhuǎn)向運(yùn)動(dòng)的時(shí)長，以躲避障礙物。（5）定義一個(gè)回調(diào)函數(shù)cbScan()，用來處理激光雷達(dá)數(shù)據(jù)。ROS每接收到一幀激光雷達(dá)數(shù)據(jù)，就會(huì)自動(dòng)調(diào)用一次回調(diào)函數(shù)。雷達(dá)的測距數(shù)值會(huì)以參數(shù)的形式傳遞到這個(gè)回調(diào)函數(shù)里。（6）在回調(diào)函數(shù)cbScan()中首先聲明兩個(gè)全局變量：一個(gè)是機(jī)器人運(yùn)動(dòng)速度的發(fā)布對象vel_pub，這個(gè)會(huì)在后面的主函數(shù)里進(jìn)行定義；另一個(gè)就是剛才定義的變量count。（7）定義一個(gè)運(yùn)動(dòng)速度消息包vel_msg，其格式為Twist。后面會(huì)發(fā)送它到機(jī)器人的核心節(jié)點(diǎn)，控制機(jī)器人的底盤運(yùn)動(dòng)。（8）在5.1節(jié)的實(shí)驗(yàn)里，我們知道回調(diào)函數(shù)cbScan()的參數(shù)msg的range是存放了激光雷達(dá)測距數(shù)值的數(shù)組，總共有360個(gè)，測距范圍為機(jī)器人周圍一圈360個(gè)方向。其中，下標(biāo)為180的測距數(shù)值對應(yīng)機(jī)器人的正前方（見圖5-20），將這個(gè)測距值保存到變量dist里，后面用來作為避障行為的激活條件。（9）調(diào)用rospy的logwarn()函數(shù)將機(jī)器人正前方的測距數(shù)值顯示在終端里。5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障

（10）為了便于理解程序流程，count數(shù)值的判斷這段先不看，調(diào)到下面對dist數(shù)值的判斷處。當(dāng)dist大于1.5米時(shí)，說明機(jī)器人正前方還沒檢測到有碰撞危險(xiǎn)的障礙物，這時(shí)候?qū)λ俣认黺el_msg的linear.x賦值0.05，讓機(jī)器人以0.05米/秒的速度繼續(xù)前進(jìn)；當(dāng)dist小于1.5米時(shí)，說明機(jī)器人正前方存在有碰撞危險(xiǎn)的障礙物，這時(shí)候?qū)λ俣认黺el_msg的angular.z賦值0.3，讓機(jī)器人以0.3弧度/秒的速度向左旋轉(zhuǎn)。同時(shí)還對count賦值50，現(xiàn)在回到前面對count的判斷代碼塊。在程序開始時(shí)，count的初值為0，只有當(dāng)機(jī)器人檢測到正前方有障礙物時(shí)，才會(huì)給count設(shè)置一個(gè)50。因此，如果count的數(shù)值大于0，說明機(jī)器人正在轉(zhuǎn)向避障當(dāng)中，系統(tǒng)會(huì)先將count的數(shù)值減1，然后return返回。這樣可以跳過后面對dist的判斷語句，避免機(jī)器人轉(zhuǎn)到一半，檢測到前方1.5米內(nèi)無障礙物后，馬上停止轉(zhuǎn)向恢復(fù)直行，這樣會(huì)有側(cè)面碰撞障礙物的風(fēng)險(xiǎn)。因?yàn)檫@里我們判斷障礙物只用到了180°這一條射線的測距值，掃描的寬度特別窄，遠(yuǎn)小于機(jī)器人的正面橫截面積，所以索性讓機(jī)器人檢測到障礙物后，直接轉(zhuǎn)一個(gè)大角度，徹底避開正前方的障礙物之后再恢復(fù)前進(jìn)直行狀態(tài)。將count設(shè)置為50，也就是這個(gè)cbScan()函數(shù)回調(diào)50次之后，才重新判斷機(jī)器人前方的障礙物，在這之前讓機(jī)器人一直處于旋轉(zhuǎn)避障狀態(tài)。5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障

（11）在回調(diào)函數(shù)cbScan()的最后，調(diào)用機(jī)器人運(yùn)動(dòng)速度發(fā)布對象vel_pub的publish()函數(shù)，將賦值后的速度消息包vel_msg發(fā)布到速度控制話題中，讓機(jī)器人的核心節(jié)點(diǎn)接收到，并控制機(jī)器人底盤執(zhí)行這個(gè)速度指令。（12）調(diào)用rospy的init_node()函數(shù)進(jìn)行該節(jié)點(diǎn)的初始化操作，參數(shù)是節(jié)點(diǎn)名稱。（13）調(diào)用rospy的Publisher()函數(shù)生成一個(gè)廣播對象vel_pub，調(diào)用的參數(shù)里指明了vel_pub將會(huì)在話題“/cmd_vel”里發(fā)布geometry_msgs::Twist類型的數(shù)據(jù)。我們對機(jī)器人的控制，就是通過這個(gè)發(fā)布消息實(shí)現(xiàn)的。（14）調(diào)用rospy的Subscriber()函數(shù)生成一個(gè)訂閱對象lidar_sub，在函數(shù)的參數(shù)中指明這個(gè)對象訂閱的是“scan”話題，數(shù)據(jù)類型為LaserScan，回調(diào)函數(shù)設(shè)置為之前定義的cbScan()，緩沖長度為10。通過這個(gè)訂閱行為，系統(tǒng)就可以在每次獲取新的激光雷達(dá)數(shù)據(jù)的時(shí)候，自動(dòng)調(diào)用一次回調(diào)函數(shù)cbScan()，從而激活一次我們編寫的避障行為代碼。（15）調(diào)用rospy的spin()對這個(gè)主函數(shù)進(jìn)行阻塞，保持這個(gè)節(jié)點(diǎn)程序不會(huì)結(jié)束退出。程序編寫完后，代碼并未馬上保存到文件里，此時(shí)會(huì)看到界面左上編輯區(qū)的文件名“behavior_node.py”右側(cè)有個(gè)白色小圓點(diǎn)（見圖5-25），這表示此文件并未保存。在按下“Ctrl+S”鍵進(jìn)行保存后，白色小圓點(diǎn)會(huì)變成關(guān)閉按鈕“×”。5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障

圖5-25文件未保存狀態(tài)5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障

2．設(shè)置可執(zhí)行權(quán)限由于這個(gè)代碼文件是新創(chuàng)建的，其默認(rèn)不帶有可執(zhí)行屬性，所以我們需要為其添加一個(gè)可執(zhí)行屬性讓它能夠運(yùn)行起來。啟動(dòng)一個(gè)終端程序，輸入如下指令進(jìn)入這個(gè)代碼文件所存放的目錄（見圖5-26）。cd~/catkin_ws/src/behavior_pkg/scripts/再輸入如下指令為代碼文件添加可執(zhí)行屬性。chmod+xbehavior_node.py設(shè)置文件權(quán)限，如圖5-27所示，按“Enter”鍵執(zhí)行后，這個(gè)代碼文件就獲得了可執(zhí)行屬性，可以在終端程序里運(yùn)行了。圖5-26進(jìn)入目錄圖5-27設(shè)置文件權(quán)限5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障

3．編譯軟件包現(xiàn)在節(jié)點(diǎn)文件可以運(yùn)行了，但是這個(gè)軟件包還沒有加入ROS的包管理系統(tǒng)，無法通過ROS指令運(yùn)行其中的節(jié)點(diǎn)，所以還需要對這個(gè)軟件包進(jìn)行編譯。在終端程序中輸入如下指令進(jìn)入ROS工作空間（見圖5-28）。cd~/catkin_ws/再輸入如下指令對軟件包進(jìn)行編譯。catkin_make編譯完成如圖5-29所示，這時(shí)就可以測試此節(jié)點(diǎn)了。圖5-28進(jìn)入ROS工作空間圖5-29編譯完成5.2.1在仿真環(huán)境中實(shí)現(xiàn)簡單避障

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

roslaunchwpr_simulationwpb_simple.launch啟動(dòng)后會(huì)彈出圖5-31所示的仿真場景，機(jī)器人位于柜子前方。圖5-30啟動(dòng)仿真場景圖5-31仿真場景5.2