輪式智能移動(dòng)操作機(jī)器人技術(shù)與應(yīng)用-基于ROS的Python編程 課件 第7章 基于代碼的導(dǎo)航應(yīng)用

第七章基于代碼的導(dǎo)航應(yīng)用

實(shí)例第6章介紹了move_base的工作流程，并且通過Rviz中的導(dǎo)航工具進(jìn)行了導(dǎo)航，本章將介紹如何通過編寫代碼的方式進(jìn)行導(dǎo)航目標(biāo)點(diǎn)的設(shè)定。與手動(dòng)在Rviz里設(shè)置操作不同，利用編寫代碼的方式設(shè)置操作需要考慮函數(shù)調(diào)用接口和ROS節(jié)點(diǎn)關(guān)系的設(shè)計(jì)。好在ROS已經(jīng)替我們考慮好了這一切，Navigation系統(tǒng)支持標(biāo)準(zhǔn)的Actionlib接口，這個(gè)接口可以讓我們?cè)诎l(fā)送導(dǎo)航終點(diǎn)坐標(biāo)之后，還能實(shí)時(shí)獲得導(dǎo)航執(zhí)行的進(jìn)度消息。只需要編寫一個(gè)ROS節(jié)點(diǎn)，就可以通過這個(gè)Actionlib接口和Navigation系統(tǒng)進(jìn)行調(diào)用和交互。目錄7.1利用編寫代碼的方式進(jìn)行導(dǎo)航7.2

開源地圖導(dǎo)航插件“Maptools”7.3本章小結(jié)7.1利用編寫代碼的方式進(jìn)行導(dǎo)航

7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航1．編寫代碼節(jié)點(diǎn)首先要?jiǎng)?chuàng)建一個(gè)ROS源碼包。在Ubuntu里打開一個(gè)終端程序，輸入如下指令進(jìn)入ROS工作空間（見圖7-1）。cdcatkin_ws/src/圖7-1進(jìn)入ROS工作空間7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航然后輸入如下指令創(chuàng)建ROS源碼包。catkin_create_pkgnav_pkgrospymove_base_msgsactionlib輸入指令按“Enter”鍵后，系統(tǒng)會(huì)提示ROS源碼包創(chuàng)建成功（見圖7-2），這時(shí)我們可以看到“catkin_ws/src”目錄下出現(xiàn)了“nav_pkg”子目錄。圖7-2創(chuàng)建nav_pkg源碼包7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航創(chuàng)建nav_pkg源碼包的指令含義，如表7-1所示。表7-1創(chuàng)建nav_pkg源碼包的指令含義指令含義catkin_create_pkg創(chuàng)建ROS源碼包（Package）的指令nav_pkg新建的ROS源碼包命名rospyPython依賴項(xiàng)，因?yàn)楸纠淌褂肞ython編寫，所以需要這個(gè)依賴項(xiàng)move_base_msgsmove_base導(dǎo)航消息依賴項(xiàng)actionlibactionlib依賴項(xiàng)，Navigation的編程接口為actionlib格式7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航接下來在VisualStudioCode中進(jìn)行操作，將目錄展開，找到前面新建的“nav_pkg”并展開，可以看到它是一個(gè)功能包最基礎(chǔ)的結(jié)構(gòu)，選中“nav_pkg”，并對(duì)其右擊，彈出圖7-3所示的快捷菜單，選擇“新建文件夾”。圖7-3快捷菜單7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航將這個(gè)文件夾命名為“scripts”（見圖7-4）。在第2章的2.3節(jié)中，有對(duì)此名稱文件夾的解釋。命名成功后按“Enter”鍵即創(chuàng)建成功。圖7-4創(chuàng)建“scripts”文件夾7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航選中此文件夾并右擊，彈出圖7-3所示的快捷菜單，選擇“新建文件”。將這個(gè)Python節(jié)點(diǎn)文件命名為“simple_goal.py”。命名完成后即可在IDE右側(cè)開始編寫“simple_goal.py”的代碼，其內(nèi)容如下。#!/usr/bin/envpython3#coding=utf-8importrospyimportactionlibfrommove_base_msgs.msgimportMoveBaseAction,MoveBaseGoalif__name__=="__main__":rospy.init_node("simple_goal")#生成一個(gè)導(dǎo)航請(qǐng)求客戶端

ac=actionlib.SimpleActionClient('move_base',MoveBaseAction)#等待服務(wù)器端啟動(dòng)

ac.wait_for_server()

7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航#構(gòu)建目標(biāo)航點(diǎn)消息

goal=MoveBaseGoal()#目標(biāo)航點(diǎn)的參考坐標(biāo)系

goal.target_pose.header.frame_id="base_footprint"#目標(biāo)航點(diǎn)在參考坐標(biāo)系里的三維數(shù)值

goal.target_pose.pose.position.x=1.0goal.target_pose.pose.position.y=0.0goal.target_pose.pose.position.z=0.0#目標(biāo)航點(diǎn)在參考坐標(biāo)系里的朝向信息

goal.target_pose.pose.orientation.x=0.0goal.target_pose.pose.orientation.y=0.0goal.target_pose.pose.orientation.z=0.0goal.target_pose.pose.orientation.w=1.0#發(fā)送目標(biāo)航點(diǎn)去執(zhí)行

ac.send_goal(goal)rospy.loginfo("開始導(dǎo)航……")ac.wait_for_result()rospy.loginfo("導(dǎo)航結(jié)束！")

7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航（1）代碼的開始部分，使用Shebang符號(hào)指定這個(gè)Python文件的解釋器為python3。如果是Ubuntu18.04或更早的版本，解釋器可設(shè)為python。（2）第二句代碼指定該文件的字符編碼為utf-8，這樣就能在代碼執(zhí)行的時(shí)候顯示中文字符。（3）使用import導(dǎo)入四個(gè)模塊，第一個(gè)是rospy模塊，包含了大部分的ROS接口函數(shù)；第二個(gè)是actionlib模塊，我們編寫的這個(gè)節(jié)點(diǎn)和ROS的導(dǎo)航系統(tǒng)是通過actionlib方式通信的；第三個(gè)是move_base_msgs消息包中的MoveBaseAction數(shù)據(jù)格式，它定義了咱們這個(gè)節(jié)點(diǎn)和ROS導(dǎo)航系統(tǒng)進(jìn)行通信的數(shù)據(jù)格式；第四個(gè)是move_base_msgs消息包中的MoveBaseGoal數(shù)據(jù)格式，用于描述導(dǎo)航目標(biāo)。7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航（4）當(dāng)__name__為“__main__”時(shí)，說明這個(gè)程序是被直接執(zhí)行的，下面就是需要執(zhí)行的內(nèi)容。（5）調(diào)用rospy的init_node()函數(shù)進(jìn)行該節(jié)點(diǎn)的初始化操作，參數(shù)是節(jié)點(diǎn)名稱。（6）調(diào)用actionlib的SimpleActionClient()函數(shù)生成一個(gè)ActionClient對(duì)象ac，用來調(diào)用和監(jiān)控ROS的導(dǎo)航服務(wù)。在SimpleActionClient()函數(shù)的參數(shù)中指定服務(wù)的名稱為“move_base”，通信格式為MoveBaseAction。MoveBaseAction通信格式如圖7-5所示。7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航圖7-5MoveBaseAction通信格式7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航這是一個(gè)包含任務(wù)目標(biāo)、執(zhí)行過程反饋及最終結(jié)果的數(shù)據(jù)結(jié)構(gòu)。其中action_goal為任務(wù)目標(biāo)，action_result為最終執(zhí)行結(jié)果，action_feedback為任務(wù)執(zhí)行過程中的狀態(tài)反饋。（7）在請(qǐng)求導(dǎo)航服務(wù)前，需要確認(rèn)導(dǎo)航服務(wù)已經(jīng)開啟，所以這里調(diào)用ac.wait_for_server()函數(shù)來查詢ROS導(dǎo)航服務(wù)的狀態(tài)。這個(gè)函數(shù)在檢測(cè)到服務(wù)沒有啟動(dòng)時(shí)會(huì)卡住不繼續(xù)往下執(zhí)行，直到檢測(cè)導(dǎo)航服務(wù)啟動(dòng)才繼續(xù)往下執(zhí)行。這個(gè)機(jī)制能避免在ROS導(dǎo)航服務(wù)還沒有啟動(dòng)時(shí)，盲目調(diào)用ROS導(dǎo)航服務(wù)導(dǎo)致任務(wù)提前失敗。（8）確認(rèn)導(dǎo)航服務(wù)啟動(dòng)后，定義一個(gè)move_base_msgs::MoveBaseGoal類型對(duì)象goal，用來傳遞我們要導(dǎo)航去的目標(biāo)信息。7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航（8）確認(rèn)導(dǎo)航服務(wù)啟動(dòng)后，定義一個(gè)move_base_msgs::MoveBaseGoal類型對(duì)象goal，用來傳遞我們要導(dǎo)航去的目標(biāo)信息。goal.target_pose.header.frame_id表示這個(gè)目標(biāo)位置的坐標(biāo)是基于哪個(gè)坐標(biāo)系的。代碼里為它賦值“base_footprint”表示這是一個(gè)基于機(jī)器人本體坐標(biāo)系的導(dǎo)航目標(biāo)位置，因?yàn)樵跈C(jī)器人的urdf描述里，“base_footprint”通常是最根部的那個(gè)joint。goal.target_pose.pose.position.x賦值1.0，表示本次導(dǎo)航的目的地是以機(jī)器人本體坐標(biāo)系為基礎(chǔ)，向X軸（機(jī)器人正前方）移動(dòng)1.0米。goal.target_pose.pose.position的y和z都賦值0。goal.target_pose.pose.orientation.w賦值1.0，其他的xyz賦值0.0，表示導(dǎo)航的目標(biāo)姿態(tài)是機(jī)器人面朝X軸的正方向（正前方）。7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航（9）調(diào)用ac.send_goal(goal)將包含導(dǎo)航目標(biāo)信息的對(duì)象傳遞給導(dǎo)航服務(wù)的客戶端ac，由ac向ROS的導(dǎo)航服務(wù)發(fā)出導(dǎo)航申請(qǐng)，并監(jiān)控后續(xù)的導(dǎo)航過程。（10）調(diào)用ac.wait_for_result()等待ROS的導(dǎo)航結(jié)果，這個(gè)函數(shù)會(huì)保持阻塞，就是卡在這，直到整個(gè)導(dǎo)航過程結(jié)束，或者導(dǎo)航過程被某些原因中斷。（11）通過rospy.loginfo()函數(shù)在終端里輸出信息，提示導(dǎo)航開始了。（12）導(dǎo)航結(jié)束后，ac.wait_for_result()的阻塞會(huì)跳出，繼續(xù)執(zhí)行后面的代碼。這里通過rospy.loginfo()函數(shù)在終端里輸出信息，提示導(dǎo)航結(jié)束。7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航程序編寫完后，代碼并未馬上保存到文件里，此時(shí)編輯區(qū)左上角的文件名“simple_goal.py”左側(cè)有個(gè)白色小圓點(diǎn)（見圖7-6），這表示此文件并未保存。在按下“Ctrl+S”鍵進(jìn)行保存后，白色小圓點(diǎn)會(huì)變成關(guān)閉按鈕“×”。圖7-6文件未保存狀態(tài)7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航2．設(shè)置可執(zhí)行權(quán)限由于這個(gè)代碼文件是新創(chuàng)建的，其默認(rèn)不帶有可執(zhí)行屬性，所以我們需要為其添加一個(gè)可執(zhí)行屬性才能讓它運(yùn)行起來。啟動(dòng)一個(gè)終端程序，輸入如下指令進(jìn)入這個(gè)代碼文件所存放的目錄（見圖7-7）。cd~/catkin_ws/src/nav_pkg/scripts/圖7-7進(jìn)入目錄7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航再執(zhí)行如下指令為代碼文件添加可執(zhí)行屬性。chmod+xsimple_goal.py按“Enter”鍵執(zhí)行后，這個(gè)代碼文件就獲得了可執(zhí)行屬性，它就可以在終端程序里運(yùn)行了（見圖7-8）。圖7-8設(shè)置文件權(quán)限7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航3．編譯軟件包現(xiàn)在節(jié)點(diǎn)文件可以運(yùn)行了，但是這個(gè)軟件包還沒有加入ROS的包管理系統(tǒng)，無法通過ROS指令運(yùn)行其中的節(jié)點(diǎn)，所以還需要對(duì)這個(gè)軟件包進(jìn)行編譯。在終端程序中輸入如下指令進(jìn)入ROS工作空間（見圖7-9）。cd~/catkin_ws/圖7-9進(jìn)入ROS工作空間7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航再輸入如下指令對(duì)軟件包進(jìn)行編譯。catkin_make編譯完成如圖7-10所示，這時(shí)就可以測(cè)試此節(jié)點(diǎn)了。圖7-10編譯完成7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航4．啟動(dòng)仿真環(huán)境啟動(dòng)開源項(xiàng)目“wpr_simulation”中的仿真場景（見圖7-11），打開終端程序，輸入如下指令。roslaunchwpr_simulationwpb_navigation.launch圖7-11啟動(dòng)仿真場景7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航仿真場景啟動(dòng)后會(huì)彈出圖7-12所示的仿真場景界面，機(jī)器人位于仿真場景的客廳入口處。圖7-12仿真場景界面7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航啟動(dòng)Gazebo的同時(shí)，Rviz也會(huì)啟動(dòng)，可以在左側(cè)任務(wù)欄進(jìn)行窗口切換，Rviz界面如圖7-13所示，在Rviz中就可以看到6.1節(jié)中創(chuàng)建好的地圖。5．標(biāo)定機(jī)器人初始位置按照第6章的方式對(duì)機(jī)器人的初始位置進(jìn)行標(biāo)定。圖7-13Rviz界面7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航6．運(yùn)行節(jié)點(diǎn)程序啟動(dòng)simple_goal節(jié)點(diǎn)，如圖7-14所示，打開一個(gè)新的終端程序，輸入如下指令。rosrunnav_pkgsimple_goal.py圖7-14啟動(dòng)simple_goal節(jié)點(diǎn)7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航此時(shí)再查看Rviz的顯示界面（見圖7-15），可以看到有一條線從機(jī)器人腳下伸向機(jī)器人正前方1米的位置（Rviz上的地面基準(zhǔn)柵格每一格表示1米距離）。圖7-15自主規(guī)劃路徑7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航此時(shí)，機(jī)器會(huì)開始向前移動(dòng)。機(jī)器人運(yùn)動(dòng)到目標(biāo)地點(diǎn)后，便會(huì)停止移動(dòng)（見圖7-16）。圖7-16到達(dá)目的地7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航此時(shí)再查看simple_goal節(jié)點(diǎn)終端，可以看到如圖7-17所示的“導(dǎo)航結(jié)束”提示。圖7-17“導(dǎo)航結(jié)束”提示7.1.1在仿真環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航7．修改節(jié)點(diǎn)參數(shù)嘗試在代碼里給goal.target_pose.pose.position的x和y設(shè)置一些不同的數(shù)值，觀察機(jī)器人的導(dǎo)航行為。8．修改節(jié)點(diǎn)坐標(biāo)系嘗試將goal.target_pose.header.frame_id改成其他坐標(biāo)系（比如“map”），觀察機(jī)器人的導(dǎo)航目的地是否發(fā)生變化。。7.1.2在真實(shí)環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航這里的程序可以在啟智ROS機(jī)器人上運(yùn)行，具體步驟如下。（1）根據(jù)啟智ROS的實(shí)驗(yàn)指導(dǎo)書對(duì)運(yùn)行環(huán)境和驅(qū)動(dòng)源碼包進(jìn)行配置。（2）連接好設(shè)備上的硬件。（3）把本章所創(chuàng)建的源碼包“nav_pkg”復(fù)制到機(jī)載計(jì)算機(jī)的“~/catkin_ws/src”目錄下進(jìn)行編譯。（4）打開機(jī)器人底盤上的電源開關(guān)（按下去）。（5）打開機(jī)器人的紅色急停開關(guān)（沿按鈕上的箭頭方向旋轉(zhuǎn)，讓其彈起），底盤電機(jī)會(huì)處于上電抱死狀態(tài)，強(qiáng)行推動(dòng)機(jī)器人會(huì)感覺到阻力。。7.1.2在真實(shí)環(huán)境中實(shí)現(xiàn)編寫代碼控制機(jī)器人導(dǎo)航（6）使用機(jī)載計(jì)算機(jī)打開終端程序，輸入如下指令。roslaunchwpb_home_tutorialsnav.launch（7）設(shè)置機(jī)器人所在的初始位置。（8）啟動(dòng)simple_goal節(jié)點(diǎn)。保持前面的導(dǎo)航服務(wù)程序繼續(xù)運(yùn)行不要退出，打開一個(gè)新的終端程序，運(yùn)行如下指令。rosrunnav_pkgsimple_goal.py按“Enter”鍵運(yùn)行后，即可看到機(jī)器人向前移動(dòng)1米。。7.2開源地圖導(dǎo)航插件“Maptools”

7.2開源地圖導(dǎo)航插件“Maptools”在地圖中使用坐標(biāo)值來表示航點(diǎn)不太直觀，在沒有測(cè)繪條件的環(huán)境下通常需要不停嘗試來調(diào)整位置。這里使用一個(gè)開源工具，可以很直觀地在地圖上設(shè)置航點(diǎn)位置，且在Rviz里將航點(diǎn)標(biāo)示出來，方便編輯查看和調(diào)用。在使用之前，需要先獲取一下開源項(xiàng)目。（1）獲取“waterplus_map_tools”開源項(xiàng)目，如圖7-18所示，在終端中輸入如下指令。cd~/catkin_ws/srcgitclone/6-robot/waterplus_map_tools.git7.2開源地圖導(dǎo)航插件“Maptools”圖7-18獲取“waterplus_map_tools”開源項(xiàng)目7.2開源地圖導(dǎo)航插件“Maptools”（2）進(jìn)行編譯。cd~/catkin_wscatkin_make開源程序編譯成功如，圖7-19所示。這樣就完成了開源項(xiàng)目的獲取，下面開始使用此插件。圖7-19開源程序編譯成功7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航1．確認(rèn)地圖文件確保已經(jīng)按照第6章步驟建立好地圖并保存到對(duì)應(yīng)文件夾。2．啟動(dòng)航點(diǎn)設(shè)置程序在仿真環(huán)境中設(shè)置航點(diǎn)，啟動(dòng)添加航點(diǎn)程序，如圖7-20所示，在終端程序中輸入如下指令。roslaunch

waterplus_map_tools

add_waypoint_simulation.launch圖7-20啟動(dòng)添加航點(diǎn)程序7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航運(yùn)行指令后可以看到圖7-21所示的Rviz界面，在界面中有前面創(chuàng)建好的地圖，并且在工具欄內(nèi)多了一個(gè)“AddWaypoint”按鈕，這個(gè)按鈕就是添加航點(diǎn)需要用到的。圖7-21Rviz界面7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航3．設(shè)置航點(diǎn)添加航點(diǎn)，單擊“AddWaypoint”按鈕，在地圖上可以用鼠標(biāo)像設(shè)置機(jī)器人初始姿態(tài)一樣設(shè)置航點(diǎn)，先單擊要設(shè)置航點(diǎn)的位置，然后按住鼠標(biāo)左鍵不放，拖動(dòng)鼠標(biāo)就可以選擇航點(diǎn)的朝向（見圖7-22）。圖7-22設(shè)置航點(diǎn)位置及朝向7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航確定好朝向后即可松開鼠標(biāo)按鍵，這時(shí)第一個(gè)航點(diǎn)添加完成（見圖7-23）。圖7-23第一個(gè)航點(diǎn)添加完成7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航每設(shè)置一個(gè)航點(diǎn)，地圖上都會(huì)顯示一個(gè)航點(diǎn)標(biāo)記，標(biāo)記的名稱是自動(dòng)生成的，可以保存后在文件中進(jìn)行修改。航點(diǎn)的位置和朝向可以在Rviz里動(dòng)態(tài)調(diào)整，如圖7-23所示，航點(diǎn)周圍共有四個(gè)箭頭和一個(gè)圓環(huán)。紅色箭頭：這一組箭頭是用來調(diào)整航點(diǎn)前后位置的，鼠標(biāo)移動(dòng)到箭頭上，這時(shí)鼠標(biāo)的光標(biāo)會(huì)發(fā)生變化，按住鼠標(biāo)左鍵并拖動(dòng)鼠標(biāo)，即可沿著示意線進(jìn)行航點(diǎn)的前后移動(dòng)（見圖7-24）。圖7-24航點(diǎn)前后移動(dòng)7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航綠色箭頭：這一組箭頭是用來調(diào)整航點(diǎn)左右位置的，鼠標(biāo)移動(dòng)到箭頭上，這時(shí)鼠標(biāo)的光標(biāo)會(huì)發(fā)生變化，按住鼠標(biāo)左鍵并拖動(dòng)鼠標(biāo)，即可沿著示意線進(jìn)行航點(diǎn)的左右移動(dòng)（見圖7-25）。圖7-25航點(diǎn)左右移動(dòng)7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航藍(lán)色圓環(huán)：這個(gè)圓環(huán)是用來調(diào)整航點(diǎn)朝向的，將鼠標(biāo)移動(dòng)到圓環(huán)上，這時(shí)鼠標(biāo)光標(biāo)會(huì)發(fā)生變化，按住鼠標(biāo)左鍵并進(jìn)行拖動(dòng)，即可繞著示意線進(jìn)行航點(diǎn)的旋轉(zhuǎn)，調(diào)整航點(diǎn)的朝向，如圖7-26所示。圖7-26調(diào)整航點(diǎn)的朝向7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航利用上面的方法，在地圖上添加更多的航點(diǎn)（見圖7-27）。圖7-27添加更多航點(diǎn)7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航需要注意的是，此時(shí)航點(diǎn)并未保存，打開一個(gè)新的終端程序并輸入如下指令進(jìn)行航點(diǎn)的保存。保存所添加的航點(diǎn)，如圖7-28所示。rosrun

waterplus_map_tools

wp_saver圖7-28保存所添加的航點(diǎn)7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航程序運(yùn)行后會(huì)在主文件夾內(nèi)生成一個(gè)名為waypoint的xml格式文件，這個(gè)文件里就保存著前面所添加的航點(diǎn)信息。對(duì)航點(diǎn)名稱的修改就需要在此文件中進(jìn)行，在每個(gè)航點(diǎn)參數(shù)內(nèi)有一對(duì)<name>參數(shù)，修改其中的內(nèi)容并保存，即可修改航點(diǎn)名稱。7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航4．編寫導(dǎo)航程序代碼創(chuàng)建好導(dǎo)航點(diǎn)就可以使用此插件進(jìn)行導(dǎo)航了，首先需要?jiǎng)?chuàng)建一個(gè)ROS源碼包。在Ubuntu里打開一個(gè)終端程序，輸入如下指令進(jìn)入ROS工作空間（見圖7-29）。cdcatkin_ws/src/圖7-29進(jìn)入ROS工作空間7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航然后輸入如下指令創(chuàng)建ROS源碼包。catkin_create_pkg

wp_pkg

rospy

move_base_msgs

actionlib

waterplus_map_tools按“Enter”鍵后創(chuàng)建wp_pkg源碼包如圖7-30所示，此時(shí)系統(tǒng)會(huì)提示ROS源碼包創(chuàng)建成功，這時(shí)可以看到“catkin_ws/src”目錄下出現(xiàn)了“wp_pkg”子目錄。圖7-30創(chuàng)建wp_pkg源碼包7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航創(chuàng)建wp_pkg源碼包的指令含義，如表7-2所示。表7-2創(chuàng)建wp_pkg源碼包的指令含義指令含義catkin_create_pkg創(chuàng)建ROS源碼包（Package）的指令wp_pkg新建的ROS源碼包命名rospyPython依賴項(xiàng)，因?yàn)楸纠淌褂肞ython編寫，所以需要這個(gè)依賴項(xiàng)move_base_msgsmove_base導(dǎo)航消息依賴項(xiàng)actionlibactionlib依賴項(xiàng)，Navigation的編程接口為actionlib格式waterplus_map_tools使用到waterplus_map_tools的航點(diǎn)信息查詢服務(wù)7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航接下來在VisualStudioCode中進(jìn)行操作，將目錄展開，找到前面新建的“wp_pkg”并展開，可以看到這是一個(gè)功能包最基礎(chǔ)的結(jié)構(gòu)，選中“wp_pkg”并對(duì)其右擊，彈出如圖7-31所示的快捷菜單，選擇“新建文件夾”。圖7-31快捷菜單7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航將這個(gè)文件夾命名為“scripts”（見圖7-32）。在第2章的2.3節(jié)中，有對(duì)此名稱文件夾的解釋。命名成功后按“Enter”鍵即可創(chuàng)建成功。圖7-32創(chuàng)建“scripts”文件夾7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航選中此文件夾并對(duì)其右擊，彈出如圖7-31所示的快捷菜單，選擇“新建文件”。將這個(gè)Python節(jié)點(diǎn)文件命名為“wp_node.py”。命名完成后即可在IDE右側(cè)開始編寫“wp_node.py”的代碼，其內(nèi)容如下。#!/usr/bin/envpython3#coding=utf-8importrospyfromstd_msgs.msgimportString#導(dǎo)航結(jié)束回調(diào)函數(shù)defresultNavi(msg):

rospy.loginfo("導(dǎo)航結(jié)果=%s",msg.data)if__name__=="__main__":

rospy.init_node("wp_node")#發(fā)布航點(diǎn)名稱話題

waypoint_pub=rospy.Publisher("/waterplus/navi_waypoint",String,queue_size=10)7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航

#訂閱導(dǎo)航結(jié)束話題

result_sub=rospy.Subscriber("/waterplus/navi_result",String,resultNavi,queue_size=10)#延時(shí)1秒鐘，讓后臺(tái)的話題發(fā)布操作能夠完成

rospy.sleep(1)#構(gòu)建航點(diǎn)名稱消息包

msg=String()

msg.data='1'#發(fā)送航點(diǎn)名稱消息包

waypoint_pub.publish(msg)#讓程序別退出，等待導(dǎo)航結(jié)果

rospy.spin()（1）代碼的開始部分，使用Shebang符號(hào)指定這個(gè)Python文件的解釋器為python3。如果是Ubuntu18.04或更早的版本，解釋器可設(shè)為python。（2）第二句代碼指定該文件的字符編碼為utf-8，這樣就能在代碼執(zhí)行的時(shí)候顯示中文字符。7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航（3）使用import導(dǎo)入兩個(gè)模塊，一個(gè)是rospy模塊，包含了大部分的ROS接口函數(shù)；另一個(gè)是字符串消息類型模塊，即std_msgs.msg里的String，我們會(huì)將導(dǎo)航目標(biāo)的航點(diǎn)名用這個(gè)消息包格式發(fā)送給導(dǎo)航系統(tǒng)。（4）首先定義一個(gè)回調(diào)函數(shù)resultNavi()，用來接收導(dǎo)航過程中反饋的狀態(tài)信息。我們的節(jié)點(diǎn)每接收到一個(gè)新的導(dǎo)航狀態(tài)，就會(huì)自動(dòng)調(diào)用一次這個(gè)回調(diào)函數(shù)。因此，我們只需要在函數(shù)里使用rospy.loginfo()函數(shù)將接收到的狀態(tài)信息顯示在終端里就行。（5）調(diào)用rospy的init_node()函數(shù)進(jìn)行該節(jié)點(diǎn)的初始化操作，參數(shù)是節(jié)點(diǎn)名稱。7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航（6）調(diào)用rospy的Publisher()函數(shù)生成一個(gè)消息發(fā)布對(duì)象waypoint_pub。調(diào)用的參數(shù)里指明了waypoint_pub將會(huì)在話題“/waterplus/navi_waypoint”里發(fā)布String類型的數(shù)據(jù)，消息的緩沖長度為10個(gè)消息包。后面我們會(huì)通過這個(gè)消息發(fā)布對(duì)象向MapTools插件系統(tǒng)發(fā)送要導(dǎo)航的目標(biāo)航點(diǎn)名稱。（7）調(diào)用rospy的Subscriber()函數(shù)生成一個(gè)訂閱對(duì)象result_sub，在函數(shù)的參數(shù)中指明這個(gè)對(duì)象訂閱的是“/waterplus/navi_result”話題，數(shù)據(jù)類型為String，回調(diào)函數(shù)設(shè)置為之前定義的resultNavi()，緩沖長度為10。（8）調(diào)用rospy.sleep()函數(shù)讓程序暫停一會(huì)，給一點(diǎn)時(shí)間，讓前面兩步話題的發(fā)布和訂閱操作在程序后臺(tái)能夠執(zhí)行完成。（5）～（8）是這個(gè)文件的主函數(shù)代碼。7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航（9）前面的準(zhǔn)備工作完成后，下面開始向?qū)Ш较到y(tǒng)發(fā)送要導(dǎo)航去的目標(biāo)航點(diǎn)名稱。構(gòu)建一個(gè)String類型的消息包msg。將目標(biāo)航點(diǎn)名稱設(shè)置到msg.data里，這里給的航點(diǎn)名稱為“1”?？梢愿鶕?jù)前面實(shí)際設(shè)置的航點(diǎn)名稱進(jìn)行修改。（10）調(diào)用waypoint_pub的publish()函數(shù)將消息包msg發(fā)送出去。MapTools插件的導(dǎo)航系統(tǒng)會(huì)接收這個(gè)消息包，并調(diào)用ROS的導(dǎo)航系統(tǒng)讓機(jī)器人完成導(dǎo)航。（11）調(diào)用rospy的spin()對(duì)這個(gè)主函數(shù)進(jìn)行阻塞，保證這個(gè)節(jié)點(diǎn)程序不會(huì)結(jié)束退出。編寫完成后，記得按“Ctrl+S”鍵進(jìn)行保存。7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航5．設(shè)置可執(zhí)行權(quán)限由于這個(gè)代碼文件是新創(chuàng)建的，其默認(rèn)不帶有可執(zhí)行屬性，所以我們需要為其添加一個(gè)可執(zhí)行屬性才能讓它運(yùn)行起來。啟動(dòng)一個(gè)終端程序，輸入如下指令進(jìn)入這個(gè)代碼文件所存放的目錄（見圖7-33）。cd~/catkin_ws/src/wp_pkg/scripts/圖7-33進(jìn)入目錄7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航再執(zhí)行如下指令為代碼文件添加可執(zhí)行屬性。chmod+xwp_node.py設(shè)置文件權(quán)限，如圖7-34所示，按“Enter”鍵執(zhí)行后，這個(gè)代碼文件就獲得了可執(zhí)行屬性，可以在終端程序里運(yùn)行了。圖7-34設(shè)置文件權(quán)限7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航6．編譯軟件包現(xiàn)在節(jié)點(diǎn)文件可以運(yùn)行了，但是這個(gè)軟件包還沒有加入ROS的包管理系統(tǒng)，無法通過ROS指令運(yùn)行其中的節(jié)點(diǎn)，因此還需要對(duì)這個(gè)軟件包進(jìn)行編譯。在終端程序中輸入如下指令進(jìn)入ROS工作空間（見圖7-35）。cd~/catkin_ws/圖7-35進(jìn)入ROS工作空間7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航再執(zhí)行如下指令對(duì)軟件包進(jìn)行編譯。catkin_make編譯完成如圖7-36所示，這時(shí)就可以測(cè)試此節(jié)點(diǎn)了。圖7-36編譯完成7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航7．啟動(dòng)仿真環(huán)境啟動(dòng)帶有導(dǎo)航插件的仿真環(huán)境，如圖7-37所示，在終端程序內(nèi)輸入如下指令。roslaunch

wpr_simulation

wpb_map_tool.launch圖7-37啟動(dòng)帶有導(dǎo)航插件的仿真環(huán)境7.2.1在仿真環(huán)境中利用導(dǎo)航插件實(shí)現(xiàn)機(jī)器人導(dǎo)航指令運(yùn)行后彈出圖7-38所示的Gazebo仿真界面。圖7-38Gaz