ROS機(jī)器人編程與SLAM算法解析指南

ROS機(jī)器人編程與SLAM算法解析指南目錄TOC\h\h第1章ROS簡介\h1.1ROS概述\h1.2Ubuntu系統(tǒng)\h1.3ROS的安裝\h1.4常用的操作命令\h第2章ROS基礎(chǔ)\h2.1開發(fā)工具IDE\h2.2節(jié)點\h2.3消息\h2.4服務(wù)\h2.5參數(shù)\h2.6動態(tài)參數(shù)設(shè)置\h2.7ROS類編程思想\h第3章調(diào)試及仿真工具\(yùn)h3.1Rviz\h3.2Gazebo\h3.3rqt的調(diào)試\h3.4rosbag的使用\h3.5rosbridge的開發(fā)\h第4章TF簡介及應(yīng)用\h4.1TF包概述\h4.2TF包的簡單使用\h4.3編寫TF發(fā)布與接收程序\h第5章SLAM簡介及應(yīng)用\h5.1SLAM概述\h5.2gmapping建圖功能應(yīng)用\h5.3ROSgmapping功能包解讀\h5.4openslam源碼解讀\h5.5ROS建圖實戰(zhàn)\h第6章ROSnavigation及算法簡介\h6.1ROSnavigationstack概述\h6.2move_base的配置\h6.3navigation源碼解讀\h6.4A-Star算法原理與實現(xiàn)\h6.5dwa算法\h第7章基于V-rep的ROS開發(fā)\h7.1V-rep機(jī)器人仿真軟件概述\h7.2V-rep安裝與ROS配置\h7.3運(yùn)行V-rep自帶ROS控制場景\h7.4V-rep環(huán)境搭建與ROS控制開發(fā)\h7.5V-rep與ROS聯(lián)合仿真實驗第1章ROS簡介1.1ROS概述ROS，全稱RobotOperatingSystem，是一個開源的機(jī)器人操作系統(tǒng)，能為異質(zhì)計算機(jī)集群提供類似操作系統(tǒng)的功能（注意：ROS不是真正意義上的操作系統(tǒng)，它通常運(yùn)行在Ubuntu系統(tǒng)上，且有固定的版本對應(yīng)）。ROS提供了操作系統(tǒng)應(yīng)有的服務(wù)，包括硬件抽象、底層設(shè)備控制、常用函數(shù)的實現(xiàn)、進(jìn)程間消息傳遞，以及包管理。ROS也提供用于獲取、編譯、編寫和跨計算機(jī)運(yùn)行代碼所需的工具和庫函數(shù)。ROS系統(tǒng)起源于2007年斯坦福大學(xué)人工智能實驗室與機(jī)器人技術(shù)公司W(wǎng)illowGarage合作的個人機(jī)器人項目PR2（PersonalRobotsProgram）。2009年初推出測試版的ROS0.4，該版本已初步具備現(xiàn)有的ROS系統(tǒng)框架。2010年正式推出ROS1.0版本，并開發(fā)出一系列機(jī)器人操作的基礎(chǔ)軟件包。之后不斷進(jìn)行版本迭代和功能完善，目前ROS的最新版本為Lunar。除此之外，支持包括Linux、Windows、macOS等操作系統(tǒng)的ROS2也已推出（本書只介紹ROS1的使用，對ROS2不作討論）。ROS各版本及其發(fā)布時間如表1-1所示。表1-1ROS版本及發(fā)布時間ROS版本發(fā)布時間LunarLoggerhead2017.5KineticKame2016.5JadeTurtle2015.5IndigoIgloo2014.7HydroMedusa2013.9GroovyGalapagos2012.12FuerteTurtle2012.4ElectricEmys2011.8Diamondback2011.3CTurtle2010.8BoxTurtleBoxTurtle2010.31.2Ubuntu系統(tǒng)Ubuntu（烏班圖）是一個基于Debian的以桌面應(yīng)用為主的Linux操作系統(tǒng)，支持x86、amd64（即x64）架構(gòu)，由全球化的專業(yè)開發(fā)團(tuán)隊（CanonicalLtd）打造。機(jī)器人操作系統(tǒng)ROS就是基于Ubuntu運(yùn)行的，因此在學(xué)習(xí)ROS之前，我們需要先花一些時間來了解如何安裝以及配置Ubuntu系統(tǒng)，為之后安裝與配置ROS系統(tǒng)做準(zhǔn)備。不同的ROS版本對應(yīng)不同版本的Ubuntu系統(tǒng)，其匹配關(guān)系如表1-2所示。表1-2ROS版本及對應(yīng)版本的Ubuntu系統(tǒng)ROS發(fā)布日期ROS版本Ubuntu系統(tǒng)版本2016.3ROSKineticKameUbuntu16.04(Xenial)/Ubuntu15.10(Wily)2015.3ROSJadeTurtleUbuntu15.04(Wily)/UbuntuLTS14.04(Trusty)2014.7ROSIndigoIglooUbuntu14.04(Trusty)2013.9ROSHydroMedusaUbuntu12.04LTS(Precise)2012.12ROSGroovyGalapagosUbuntu12.041.2.1Ubuntu系統(tǒng)的安裝準(zhǔn)備工作如下。Ubuntu的鏡像文件。U盤，用于制作啟動盤。UltraISO軟件，用于刻錄啟動U盤。進(jìn)入Ubuntu官網(wǎng)/download/alternative-downloads下載安裝包，界面顯示可供下載的Ubuntu鏡像選項如下。Downloadthenetworkinstallerfor18.10Downloadthenetworkinstallerfor18.04LTSDownloadthenetworkinstallerfor16.04LTSDownloadthenetworkinstallerfor14.04LTS選擇下載對應(yīng)版本（本書以14.04為例），后續(xù)對應(yīng)ROS的Indigo版本使用。為電腦分出30GB～70GB的存儲空間，從現(xiàn)有的硬盤中直接壓縮。具體操作是，右擊“計算機(jī)→管理→磁盤管理”，可以很清楚地看到各個磁盤的分區(qū)情況，右擊選中待壓縮的磁盤，單擊壓縮卷，壓縮出30GB～70GB的內(nèi)存用于安裝Ubuntu系統(tǒng)。完成分區(qū)之后開始刻錄U盤啟動盤，安裝下載好的UltraISO軟件并打開，如圖1-1所示，在菜單里找到“啟動”選項，單擊“寫入硬盤映像”。圖1-1UltraISO軟件啟動界面在彈出的窗口中單擊“便捷啟動”選項，在下拉菜單中選擇“寫入新的硬盤主引導(dǎo)記錄（MBR）”，繼續(xù)選擇“USB-HDD+”，如圖1-2所示。圖1-2寫入磁盤映像寫入完成后，拔出U盤并關(guān)閉電腦，然后重新插入U盤，啟動電腦，通過快捷鍵進(jìn)入BIOS，選擇U盤啟動。各電腦進(jìn)入BIOS的快捷鍵可參考表1-3。表1-3BIOS快捷鍵參考筆記本啟動按鍵臺式機(jī)啟動按鍵聯(lián)想筆記本F12聯(lián)想臺式機(jī)F12宏基筆記本F12惠普臺式機(jī)F12華碩筆記本Esc宏基臺式機(jī)F12惠普筆記本F9戴爾臺式機(jī)Esc聯(lián)想ThinkPadF12神舟臺式機(jī)F12戴爾筆記本F12華碩臺式機(jī)F8神舟筆記本F12方正臺式機(jī)F12東芝筆記本F12清華同方臺式機(jī)F12三星筆記本F12明基臺式機(jī)F8IBM筆記本F12進(jìn)入BIOS之后，在安裝界面選擇“中文（簡體）”，如圖1-3所示，單擊“安裝Ubuntu”。圖1-3Ubuntu安裝界面在準(zhǔn)備安裝Ubuntu界面中單擊“繼續(xù)”，如圖1-4所示。圖1-4準(zhǔn)備安裝界面在新彈出的安裝類型界面中選擇“其他選項”，單擊“繼續(xù)”，如圖1-5所示。圖1-5安裝類型界面在彈出的新建分區(qū)界面中，單擊“新建分區(qū)表...”，如圖1-6所示。圖1-6新建分區(qū)界面此時，彈出窗口提示“要在此設(shè)備上創(chuàng)建新的空分區(qū)表嗎？”，選擇“繼續(xù)”，如圖1-7所示。圖1-7是否新建分區(qū)表界面接下來，建立/boot分區(qū)。如圖1-8所示，單擊“空閑”磁盤分區(qū)，單擊“+”添加新分區(qū)，將大小設(shè)置為500MB，類型為“主分區(qū)”，掛載點為“/boot”，最后單擊“確定”。圖1-8新建boot分區(qū)接下來，新建交換空間分區(qū)。如圖1-9所示，單擊“空閑”磁盤分區(qū)，單擊“+”添加新分區(qū)，將大小設(shè)置為2048MB，類型為“主分區(qū)”，用于“交換空間”，最后單擊“確定”。圖1-9新建交換空間分區(qū)然后，新建根分區(qū)。如圖1-10所示，單擊“空閑”磁盤分區(qū)，單擊“+”添加新分區(qū)，將大小設(shè)置為50GB，類型為“主分區(qū)”，掛載點為“/”根分區(qū)，最后單擊“確定”。圖1-10新建根分區(qū)接著，新建home分區(qū)。單擊“空閑”磁盤分區(qū)，單擊“+”添加新分區(qū)，將大小設(shè)置為剩余全部空間，類型為“邏輯分區(qū)”，掛載點為“/home”，單擊“確定”，最后單擊“現(xiàn)在安裝”，如圖1-11所示。圖1-11新建home分區(qū)接下來，在頁面中設(shè)置用戶名、計算機(jī)名及密碼，單擊“繼續(xù)”，安裝完成后單擊“現(xiàn)在重啟”，如圖1-12所示。圖1-12是否重啟界面重啟后，可看到圖1-13所示的安裝成功界面。圖1-13安裝成功界面至此，Ubuntu系統(tǒng)在電腦上的安裝工作完成。1.2.2樹莓派安裝Ubuntu1．準(zhǔn)備工作（1）樹莓派：HDMI轉(zhuǎn)VGA轉(zhuǎn)接線（連接樹莓派與顯示器）、電源（樹莓派供電）、microSD卡（安裝系統(tǒng)）。.img鏡像的官方下載地址為/ubuntu/releases/。（2）Win32diskmanager（鏡像讀寫工具）的官方下載地址為/projects/win32diskimager/。2．安裝步驟（1）將microSD卡插入讀卡器。（2）打開Win32DiskImager，選擇下載的.img鏡像及microSD卡的盤符，然后寫入。（3）將寫好的系統(tǒng)內(nèi)存卡插入樹莓派，然后將鍵盤、鼠標(biāo)、顯示器、樹莓派插上電源即可啟動。（4）設(shè)置系統(tǒng)語言、用戶名和密碼等。1.3ROS的安裝配置Ubuntu軟件倉庫，打開軟件和更新對話框，具體可以在Ubuntu最左上角的“搜索”選項中搜索。打開后按照圖1-14所示進(jìn)行配置（確保勾選了“restricted”“universe”和“multiverse”）。圖1-14配置Ubuntu軟件倉庫添加sources.lists，配置電腦使其能夠安裝來自的軟件。ROSIndigo僅支持Saucy(13.10)和Trusty(14.04)：sudosh-c'echo"deb/ros/ubuntu$(lsb_release-sc)main">/etc/apt/sources.list.d/ros-latest.list'

添加keys：sudoapt-keyadv--keyserverhkp://--recv-key421C365BD9FF1F717815A3895523BAEEB01FA116

更新Debian軟件包索引：sudoapt-getupdate

安裝桌面完整版ROS：sudoapt-getinstallros-indigo-desktop-full

在開始使用ROS之前，還需要初始化rosdep。rosdep可以在編譯某些源碼時為其安裝一些系統(tǒng)依賴，也是某些ROS核心功能組件必須要用到的工具。sudorosdepinit

rosdepupdate

下面開始配置環(huán)境。打開終端，輸入以下內(nèi)容：$echo"source/opt/ros/kinetic/setup.bash">>~/.bashrc

$source~/.bashrc

安裝rosinstall。rosintall是ROS中經(jīng)常使用的命令行，可以便于我們在一行命令中同時下載多個ROSpackages，其安裝命令如下：sudoapt-getinstallpython-rosinstall

測試時，在命令行輸入“roscore”啟動ROS節(jié)點管理器，如圖1-15所示。圖1-15ROS節(jié)點管理器啟動界面如果看到圖1-15所示的結(jié)果，說明已成功安裝ROS。1.4常用的操作命令1.4.1Ubuntu系統(tǒng)的常用命令由于ROS是基于Ubuntu系統(tǒng)運(yùn)行的，因此在學(xué)習(xí)ROS之前，我們需要熟悉一些基本的Linux操作指令。常用的操作命令分為以下3類。（1）查找指令ls：列出當(dāng)前目錄下的所有文件（不顯示隱藏文件）。ls–a：列出當(dāng)前目錄下的所有文件（顯示隱藏文件）。pwd：顯示當(dāng)前路徑。cat/etc/issue：查看Ubuntu版本。sudofdisk–l：查看磁盤信息。free–m：查看當(dāng)前的內(nèi)存使用情況。ps–A：查看當(dāng)前有哪些進(jìn)程。（2）文件/文件夾操作命令cd~/file_path：轉(zhuǎn)到（切換）路徑。mkdirdirname：新建目錄。rmdirdirname：刪除空目錄。rmfilename：刪除文件。rm–rfdirname：刪除非空目錄及其包含的所有文件。mvfile1file2：將文件1重命名為文件2。mvfile1dir1：將文件1移動到目錄1。（3）安裝軟件及創(chuàng)建用戶命令sudoapt-getinstallxxx：安裝程序。sudoapt-getremove–purgexxx：徹底卸載程序并清空配置。sudoapt-getupdate：更新本地軟件源文件。sudouseraddusername：創(chuàng)建一個新的用戶username。sudopasswdusername：設(shè)置用戶username的密碼。sudogroupaddgroupname：創(chuàng)建一個新的groupname組。sudousermod–ggroupnameusername：把用戶username加入groupname組中。1.4.2常用的ROS操作命令ROS中包含豐富的調(diào)試命令。本書只提供一些常用的基本操作命令，這些命令會在后續(xù)章節(jié)中頻繁地被使用，希望讀者熟記。（1）工作空間及功能包相關(guān)命令rosc：切換（cd）工作目錄到某個程序包（或其子目錄）roscd[package[/subdir]]。rosls：按程序包的名稱執(zhí)行l(wèi)s命令rosls[package[/subdir]]。catkin_create_pkg：創(chuàng)建功能包。catkin_make：編譯ROS工作空間。（2）節(jié)點啟動及主題調(diào)試命令roscore：啟動管理器。rosrun：運(yùn)行ROS包中的一個可執(zhí)行文件rosrunpackage_nameexecutable_name。ruslaunch：啟動roscore、本地節(jié)點和遠(yuǎn)程節(jié)點（通過ssh），設(shè)置參數(shù)服務(wù)器的參數(shù)。roslaunchpackage_namefile_name.launch：啟動包中的一個文件。rospack：獲取程序包的有關(guān)信息。rospackfind[package]：返回程序包的路徑。rospacklist：獲取所有的程序包。rosdep：一個能夠下載并安裝ROSpackages所需要的系統(tǒng)依賴項的工具rosdepinstall[package]。roswtf：可以檢查ROS系統(tǒng)并嘗試發(fā)現(xiàn)問題，如roswtforroswtf[file]。rostopic–h：查看rostopic的所有操作。rostopiclist：查看所有Topic列表。rosrunrqt_plotrqt_plot：圖形化顯示Topic。rostopicecho[topic]：查看某個Topic信息。第2章ROS基礎(chǔ)2.1開發(fā)工具IDERoboWareStudio是一個ROS集成開發(fā)環(huán)境，使ROS開發(fā)更加直觀、簡單，并且易于操作，可用于ROS工作區(qū)及包的管理，代碼編輯、構(gòu)建及調(diào)試。RoboWareStudio專為ROS（indigo/jade/kinetic）設(shè)計，以圖形化的方式完成ROS工作區(qū)及包的創(chuàng)建、源碼添加、message/service/action文件創(chuàng)建、顯示包及節(jié)點列表，并且可以實現(xiàn)CMakelists.txt文件和package.xml文件的自動更新。下面介紹該開發(fā)環(huán)境的安裝與使用。2.1.1RoboWareStudio的安裝1．準(zhǔn)備工作（1）操作系統(tǒng)為Ubuntu。（2）已完成ROS的安裝配置。（3）可以使用catkin_make構(gòu)建ROS包（若無法構(gòu)建，可以運(yùn)行下列命令來安裝構(gòu)建工具）：sudoapt-getinstallbuild-essential

（4）安裝python-pip：sudoapt-getinstallpython-pip

2．安裝進(jìn)入RoboWare官網(wǎng)，界面如圖2-1所示。圖2-1RoboWare官網(wǎng)界面選擇下載對應(yīng)的RoboWareStudio版本，雙擊下載下來的.deb文件即可完成安裝。安裝過程中若彈出用戶協(xié)議，則直接按<Esc>鍵；若彈出“您是否接受上述協(xié)議？”窗口，則單擊“確定”，按回車鍵，自動開始安裝，如圖2-2所示。圖2-2用戶協(xié)議界面安裝成功后，可看到圖2-3所示的工作界面。圖2-3工作界面2.1.2卸載打開任意一個終端，執(zhí)行以下指令來卸載RoboWareStudio：sudoapt-getremoveroboware-studio

2.1.3使用1．創(chuàng)建工作區(qū)在歡迎界面中單擊“新建工作區(qū)”（或選擇“文件→新建工作區(qū)”），選擇路徑并填寫工作區(qū)名稱，如“catkin_ws”，則會創(chuàng)建一個名為“catkin_ws”的工作區(qū)，并顯示在資源管理器窗口中，如圖2-4所示。圖2-4創(chuàng)建工作區(qū)2．創(chuàng)建ROS包右擊ROS工作區(qū)下的“src”，選擇“新建ROS包”，輸入包名稱及其依賴包的名稱。按回車鍵后，會創(chuàng)建對應(yīng)的ROS包，如圖2-5所示。圖2-5創(chuàng)建ROS包右擊ROS包目錄下的“src”，選擇“新建CPP源文件”，輸入文件名后，按回車鍵，會彈出列表。在列表中選擇類型“加入新的可執(zhí)行文件中”，則會創(chuàng)建一個與CPP文件同名的執(zhí)行文件（ROS節(jié)點），此時CMakeLists.txt文件會自動更新。右擊包名文件夾，依次選擇“新建Msg文件夾”“新建Srv文件夾”“新建Action文件夾”，可分別創(chuàng)建Message、Service、Action文件夾，右擊相應(yīng)文件夾即可添加Message、Service、Action文件。此時CMakeLists.txt文件會自動更新，如圖2-6所示。圖2-6新建Msg文件夾3．編譯文件單擊左上角的小錘子圖標(biāo)，進(jìn)行編譯文件，或者通過在菜單欄中單擊“ROS→build”進(jìn)行編譯。4．添加并啟動Launch文件首先，右擊包名文件夾（如“my_package”），選擇“新建Launch文件夾”來創(chuàng)建Launch文件夾，如圖2-7所示。圖2-7新建Launch文件夾然后，右擊Launch文件夾，選擇“新建LAUNCH文件”，輸入文件名添加Launch文件，如圖2-8所示。圖2-8新建Launch文件編輯完成后，右擊Launch文件，選擇“運(yùn)行Launch文件”，RoboWareStudio就會自動打開集成終端并運(yùn)行Launch文件，如圖2-9所示。圖2-9運(yùn)行Launch文件在菜單欄中，依次選擇“ROS-運(yùn)行roscore”“ROS-運(yùn)行RViz”“ROS-運(yùn)行rqt”“ROS-運(yùn)行rqt-reconfigure”“ROS-運(yùn)行rqt-graph”，可分別啟動roscore、RViz、rqt、rqt-reconfigure、rqt-graph。圖2-10所示為啟動rqt-reconfigure的界面。圖2-10啟動rqt-reconfigure5．導(dǎo)入已有的ROS工作區(qū)（1）對于普通的ROS工作區(qū)，直接在歡迎界面單擊“打開工作區(qū)”按鈕，或在菜單欄中選擇“文件→打開工作區(qū)”，選擇工作區(qū)路徑打開。（2）對于舊版本RoboWareStudio打開過的ROS工作區(qū)，需要將工作區(qū)根目錄下的“.vscode”文件夾刪除，再打開工作區(qū)。6．修改界面語言在菜單欄中，選擇“文件→首選項→語言設(shè)置”，打開配置文件，如圖2-11所示。圖2-11語言設(shè)置"locale":"zh-CN"表示設(shè)置為中文界面，"locale":"en"表示設(shè)置為英文界面，可用“//”進(jìn)行注釋。修改完成后，重啟RoboWareStudio即可生效。這里我們只介紹RoboWareStudio的基本常用方法。讀者若對軟件的具體使用感興趣，可自行查閱RoboWareStudio軟件使用手冊。2.2節(jié)點在ROS中，最小的進(jìn)程單位稱為節(jié)點（Node）。每個節(jié)點都是一個可執(zhí)行文件，負(fù)責(zé)實現(xiàn)具體的功能，如激光數(shù)據(jù)獲取處理、坐標(biāo)變換、圖像信息獲取處理里程及發(fā)布等。構(gòu)成節(jié)點的可執(zhí)行文件通常由C++和Python等語言編寫。節(jié)點通過在具體主題上發(fā)布對應(yīng)消息與其他節(jié)點進(jìn)行通信。此外，ROS也通過節(jié)點管理器roscore來管理運(yùn)行中的各節(jié)點。下面以接收端和發(fā)布端為例，通過編寫相應(yīng)節(jié)點來闡述ROS節(jié)點間的通信原理。2.2.1發(fā)布端打開編輯器，創(chuàng)建發(fā)布端PubForBeginner.cpp，內(nèi)容如下：#include"ros/ros.h"

#include"std_msgs/String.h"

intmain(intargc,char**argv)

{

//initializeandnamenode

ros::init(argc,argv,"publisher");//createnodehandle

ros::NodeHandlenh;

//createpublisher

ros::Publishersimplepub=nh.advertise<std_msgs::String>("string_topic",100);

//publishfrequency

ros::Raterate(10);

//messageforpublish

std_msgs::Stringpubinfo;

pubinfo.data="Hello,I'mPublisher!";

while(ros::ok())

{

simplepub.publish(pubinfo);

rate.sleep();

}

return0;

}

說明如下。ros::init(argc,argv,"publisher")：創(chuàng)建ros節(jié)點，并命名為publisher。ros::NodeHandlenh：創(chuàng)建rosnodehandle句柄。ros::Publishersimplepub=nh.advertise<std_msgs::String>("string_topic",100)：創(chuàng)建發(fā)布者，并在string_topic主題上發(fā)布std_msgs::String類型的消息。pubinfo.data="Hello,I'mPublisher!"：消息內(nèi)容。simplepub.publish(pubinfo)：結(jié)合while和ros::Rate按照固定頻率循環(huán)發(fā)布消息。2.2.2接收端打開編輯器，創(chuàng)建接收端SubForBeginner.cpp，內(nèi)容如下：#include"ros/ros.h"

#include"std_msgs/String.h"

usingnamespacestd;

voidsubCallback(conststd_msgs::String&submsg)

{

stringsubinfo;

subinfo=submsg.data;

ROS_INFO("Themessagesubscribedis:%s",subinfo.c_str());

}

intmain(intargc,char**argv)

{

//initialandnamenode

ros::init(argc,argv,"subscriber");

//createnodehandle

ros::NodeHandlenh;

//createsubscriber

ros::Subscribersub=nh.subscribe("string_topic",1000,&subCallback);

ros::spin();

return0;

}

說明如下。voidsubCallback(conststd_msgs::String&submsg)：訂閱器回調(diào)函數(shù)，對接收到的消息進(jìn)行打印處理。ros::Subscribersub=nh.subscribe("string_topic",1000,&subCallback)：訂閱器，訂閱string_topic主題上的消息，并通過回調(diào)函數(shù)進(jìn)行處理。ros::spin()：ROS消息回調(diào)處理函數(shù)，程序運(yùn)行到這里后不再往下執(zhí)行。2.2.3CMakeLists.txt文件在CMakeLists.txt文件中添加如下內(nèi)容：cmake_minimum_required(VERSION2.8.3)

project(book_node)

find_package(catkinREQUIREDCOMPONENTSroscpprospystd_msgs)

catkin_package(

)

include_directories(

include${catkin_INCLUDE_DIRS}

)

add_executable(PubForBeginner

src/PubForBeginner.cpp

)

add_dependencies(PubForBeginner${${PROJECT_NAME}_EXPORTED_TARGETS}${catkin_EXPORTED_TARGETS})

target_link_libraries(PubForBeginner

${catkin_LIBRARIES}

)

add_executable(SubForBeginner

src/SubForBeginner.cpp

)

add_dependencies(SubForBeginner${${PROJECT_NAME}_EXPORTED_TARGETS}${catkin_EXPORTED_TARGETS})

target_link_libraries(SubForBeginner

${catkin_LIBRARIES}

)

編譯即可獲得PubForBeginner和SubForBeginner可執(zhí)行文件。2.2.4測試打開兩個終端分別運(yùn)行：rosrunbook_nodeSubForBeginner

rosrunbook_nodePubForBeginner

可以在接收端看到如下打印信息：[INFO][1551682316.518305766]:Themessagesubscribedis:Hello,I'mPublisher!

[INFO][1551682316.618326302]:Themessagesubscribedis:Hello,I'mPublisher!

[INFO][1551682316.718301812]:Themessagesubscribedis:Hello,I'mPublisher!

在新終端運(yùn)行rosrunrqt_graphrqt_graph，可以查看節(jié)點間的通信關(guān)系，如圖2-12所示。圖2-12節(jié)點間的通信關(guān)系圖2-12清晰地表明，publisher節(jié)點和subscriber節(jié)點通過string_topic主題成功實現(xiàn)通信。2.3消息ROS中有許多已定義的消息類型，如int32、int64、float64和string等。但是為了滿足不同場景功能需求，通常需要用戶自定義消息類型。下面我們將練習(xí)如何自定義消息類型，并進(jìn)行發(fā)布和接收。2.3.1自定義消息在RoboWareStudio創(chuàng)建通過AddMsgFolder創(chuàng)建msg文件夾，通過AddMsgFile創(chuàng)建Student.msg文件，內(nèi)容如下。stringnamefloat64heightfloat64weight該消息主要包含3部分。字符串類型的學(xué)生姓名信息。float64類型的學(xué)生身高信息。float64類型的學(xué)生體重信息。在CMakeLists.txt文件中添加如下內(nèi)容：find_package(catkinREQUIREDCOMPONENTS

message_generationroscpprospystd_msgs)

add_message_files(FILES

Student.msg

)

generate_messages(DEPENDENCIES

std_msgs

)

編譯之后，在.../robware_ws/devel/include/book_message文件夾下可看到自動生成的頭文件：Student.h。2.3.2編寫自定義消息發(fā)布端新建文件Book_MyMsgPub.cpp，內(nèi)容如下：#include"ros/ros.h"

#include"book_message/Student.h"

#include<cstdlib>

usingnamespacestd;

intmain(intargc,char**argv)

{

//initialandnamenode

ros::init(argc,argv,"node_MyMsgPub");

if(argc!=4)

{

cout<<"Errorcommandparameter!\n"\

<<"Pleaseruncommandeg:\n"\

<<"rosrunbook_messgeBook_MyMsgPubLiLei180160"<<endl;

return1;

}

//createnodehandle

ros::NodeHandlenh;

//createmessagepublisher

ros::PublisherMyMsgPub=nh.advertise<book_message::Student>("MyMsg",100);

book_message::StudentsdtMsg;

sdtM=argv[1];

//convertstringinargvtofloatandpassvaluetoheight,weight

sdtMsg.height=atof(argv[2]);

sdtMsg.weight=atof(argv[3]);

ros::Raterate(10);

while(ros::ok())

{

MyMsgPub.publish(sdtMsg);

rate.sleep();

}

return0;

}

說明如下。ros::init(argc,argv,"node_MyMsgPub")：初始化并命名節(jié)點名稱。if(argc!=4)：該段代碼的主要功能是提醒使用者如何使用該節(jié)點。ros::PublisherMyMsgPub=nh.advertise<book_message::Student>("MyMsg",100)：在MyMsg主題上發(fā)布用戶自定義的book_message::Student類型消息。book_message::StudentsdtMsg：定義將發(fā)布的消息。sdtM=argv[1]：命令行第二個參數(shù)作為學(xué)生的名字信息。sdtMsg.height=atof(argv[2])：命令行第三參數(shù)作為學(xué)生的身高信息（atof()函數(shù)將字符串轉(zhuǎn)化為浮點數(shù)）。sdtMsg.weight=atof(argv[3])：命令行第四個參數(shù)作為學(xué)生的體重信息。MyMsgPub.publish(sdtMsg)：循環(huán)體中以固定頻率發(fā)布消息。2.3.3編寫自定義消息接收端創(chuàng)建接收端文件Book_MyMsgSub.cpp，內(nèi)容如下：#include"ros/ros.h"

#include"book_message/Student.h"

//customdefinedmessagecallbackfunction

voidMyMsgCallback(constbook_message::Student&sdtInfo)

{

ROS_INFO("Thestudentinformationis:\nname:%sheight:%fweight:%f",

sdtI.c_str(),

sdtInfo.height,

sdtInfo.weight);

}

intmain(intargc,char**argv)

{

//initialandnamenode

ros::init(argc,argv,"node_MyMsgSub");

//createnodehandle

ros::NodeHandlenh;

//createsubscriber

ros::SubscriberMyMsgSub=nh.subscribe("MyMsg",1000,&MyMsgCallback);

ros::spin();

return0;

}

類似之前創(chuàng)建的接收端程序，不過這里使用的是自定義的學(xué)生信息的數(shù)據(jù)類型。說明如下。voidMyMsgCallback(constbook_message::Student&sdtInfo)：回調(diào)函數(shù)，對接收到的學(xué)生信息進(jìn)行打印處理。ros::SubscriberMyMsgSub=nh.subscribe("MyMsg",1000,&MyMsgCallback)：創(chuàng)建接收者，并通過回調(diào)函數(shù)接收和處理MyMsg主題上的消息。2.3.4CMakeLists.txt文件最終的CMakeLists.txt文件內(nèi)容如下：cmake_minimum_required(VERSION2.8.3)

project(book_message)

find_package(catkinREQUIREDCOMPONENTS

message_generationroscpprospystd_msgs)

add_message_files(FILES

Student.msg

)

generate_messages(DEPENDENCIES

std_msgs

)

catkin_package(

CATKIN_DEPENDS

message_runtime

)

include_directories(

include${catkin_INCLUDE_DIRS}

)

add_executable(Book_MyMsgPub

src/Book_MyMsgPub.cpp

)

add_dependencies(Book_MyMsgPub${${PROJECT_NAME}_EXPORTED_TARGETS}${catkin_EXPORTED_TARGETS})

target_link_libraries(Book_MyMsgPub

${catkin_LIBRARIES}

)

add_executable(Book_MyMsgSub

src/Book_MyMsgSub.cpp

)

add_dependencies(Book_MyMsgSub${${PROJECT_NAME}_EXPORTED_TARGETS}${catkin_EXPORTED_TARGETS})

target_link_libraries(Book_MyMsgSub

${catkin_LIBRARIES}

)

編譯獲得可執(zhí)行文件Book_MyMsgPub和Book_MyMsgSub。2.3.5測試在兩個終端分別輸入如下內(nèi)容：rosrunbook_messageBook_MyMsgPubLiLei180160

rosrunbook_messageBook_MyMsgSub

可看到如下打印信息：[INFO][1551684824.756760205]:Thestudentinformationis:

name:LiLeiheight:180.000000weight:160.000000

[INFO][1551684824.856785386]:Thestudentinformationis:

name:LiLeiheight:180.000000weight:160.000000

[INFO][1551684824.956813035]:Thestudentinformationis:

name:LiLeiheight:180.000000weight:160.000000

在新終端運(yùn)行rosrunrqt_graphrqt_graph，查看節(jié)點關(guān)系圖，如圖2-13所示。圖2-13自定義消息節(jié)點圖2.4服務(wù)2.4.1服務(wù)通信在ROS中，除了消息這種通信類型外，還有一種稱為服務(wù)的通信類型。不同于消息通信是單向的，服務(wù)是一種雙向的通信，可以對接收到的請求做出回應(yīng)。接下來練習(xí)如何使用自定義的消息類型進(jìn)行通信測試，在本例中，創(chuàng)建一個服務(wù)文件，用于存儲請求內(nèi)容（輸入密碼）和返回的結(jié)果（密碼是否正確）。2.4.2自定義srv在RoboWareStudio中，通過AddSrvFolder創(chuàng)建srv文件夾，通過AddSrvFile創(chuàng)建PassWord.srv文件，內(nèi)容如下：int64password

boolresult

該srv文件主要包含請求與響應(yīng)部分。password為int64類型的請求內(nèi)容。result是bool類型的響應(yīng)。類似自定義消息，在CMakeLists.txt文件中添加如下內(nèi)容：find_package(catkinREQUIREDCOMPONENTSroscpprospystd_msgsmessage_generation)

add_service_files(FILES

PassWord.srv

)

generate_messages(DEPENDENCIES

std_msgs

)

編譯之后，在.../robware_ws/devel/include/book_service文件夾下可獲得3個自動生成的文件：PassWord.h、PassWordRequest.h、PassWordResponse.h。2.4.3創(chuàng)建服務(wù)器新建文件ServerForBeginner.cpp，內(nèi)容如下：#include"ros/ros.h"

#include"book_service/PassWord.h"

//servercallbackfunction

boolserverCallback(book_service::PassWord::Request&req,

book_service::PassWord::Response&res)

{

//ifpassword=123456,resultistrue,orresultisfalse

res.result=(req.password==123456)?true:false;

returntrue;

}

intmain(intargc,char**argv)

{

//initialandnamenode

ros::init(argc,argv,"server_node");

//createnodehandle

ros::NodeHandlenh;

//createserviceserver

ros::ServiceServerserv=nh.advertiseService("pswserver",&serverCallback);

ros::spin();

return0;

}

說明如下。boolserverCallback(book_service::PassWord::Request&req，book_service::PassWord::Response&res)：服務(wù)端回調(diào)函數(shù)，對客戶端發(fā)出的請求做出相應(yīng)的響應(yīng)，在本例中會根據(jù)客戶端發(fā)出的密碼做出密碼正確或錯誤的響應(yīng)。ros::ServiceServerserv=nh.advertiseService("pswserver",&serverCallback)：創(chuàng)建服務(wù)器，并通過回調(diào)函數(shù)對請求做出響應(yīng)。2.4.4創(chuàng)建客戶端新建文件ClientForBeginner.cpp，內(nèi)容如下：#include"ros/ros.h"

#include"book_service/PassWord.h"

#include<cstdlib>

usingnamespacestd;

intmain(intargc,char**argv)

{

//initialandnamenode

ros::init(argc,argv,"node_client");

if(argc<2)

{

cout<<"Errorparamster,pleaseruneg:rosrunbook_serviceClientForBeginner123456"<<endl;

return1;

}

//createnodehandle

ros::NodeHandlenh;

//createclient

ros::ServiceClientclient=nh.serviceClient<book_service::PassWord>("pswserver",100);

book_service::PassWordsrv;

//convertargvfromchartointandpassvaluetoserviceresponse

srv.request.password=atoi(argv[1]);

if(client.call(srv))

{

ROS_INFO("clientconnectsuccess!");

if(srv.response.result)

{

ROS_INFO("Welcom,correctpassword!");

}else{

ROS_INFO("Sorry,passworderror!");

}

}else{

ROS_INFO("clientconnetfail!");

}

return0;

}

說明如下。if(argc<2)：如果命令參數(shù)錯誤，提示用戶如何正確填寫命令行。ros::ServiceClientclient=nh.serviceClient<book_service::PassWord>("pswserver",100)：創(chuàng)建客戶端。book_service::PassWordsrv：創(chuàng)建服務(wù)。srv.request.password=atoi(argv[1])：命令行第二個參數(shù)賦值給服務(wù)的請求。if(client.call(srv))：如果通信成功，打印success，否則打印fail。if(srv.response.result)：根據(jù)服務(wù)器響應(yīng)打印密碼正確或錯誤的信息。2.4.5CMakeLists.txt文件在CMakeLists.txt文件中添加如下內(nèi)容：cmake_minimum_required(VERSION2.8.3)

project(book_service)

find_package(catkinREQUIREDCOMPONENTSroscpprospystd_msgsmessage_generation)

add_service_files(FILES

PassWord.srv

)

generate_messages(DEPENDENCIES

std_msgs

)

catkin_package(

CATKIN_DEPENDS

message_runtime

)

include_directories(

include${catkin_INCLUDE_DIRS}

)

add_executable(ServerForBeginner

src/ServerForBeginner.cpp

)

add_dependencies(ServerForBeginner${${PROJECT_NAME}_EXPORTED_TARGETS}${catkin_EXPORTED_TARGETS})

target_link_libraries(ServerForBeginner

${catkin_LIBRARIES}

)

add_executable(ClientForBeginner

src/ClientForBeginner.cpp

)

add_dependencies(ClientForBeginner${${PROJECT_NAME}_EXPORTED_TARGETS}${catkin_EXPORTED_TARGETS})

target_link_libraries(ClientForBeginner

${catkin_LIBRARIES}

)

編譯獲得可執(zhí)行文件ClientForBeginner和ServerForBeginner。2.4.6測試在不同終端分別運(yùn)行如下代碼：rosrunbook_serviceClientForBeginner123456

rosrunbook_serviceServerForBeginner

可看到如下打印內(nèi)容：[INFO][1551686984.811029702]:clientconnectsuccess!

[INFO][1551686984.811119481]:Welcom,correctpassword!

如果運(yùn)行如下代碼：rosrunbook_serviceClientForBeginner

可看到如下提示信息：Errorparamster,pleaseruneg:rosrunbook_serviceClientForBeginner123456

若輸入錯誤密碼，如：rosrunbook_serviceClientForBeginner12345

可得到如下打印信息：[INFO][1551687286.099570151]:clientconnectsuccess!

[INFO][1551687286.099667373]:Sorry,passworderror!

2.5參數(shù)ROS中通常包含許多參數(shù)，這些參數(shù)在可執(zhí)行程序中起到了非常關(guān)鍵的作用。ROS為我們提供了3種不同的方式來設(shè)置和獲取參數(shù)，接下來將練習(xí)如何編寫可執(zhí)行文件，通過不同的方法來設(shè)置和獲取參數(shù)。2.5.1編寫參數(shù)設(shè)置獲取節(jié)點參數(shù)的設(shè)置和獲取通?？梢酝ㄟ^命令行實現(xiàn)，但是為了便于維護(hù)和排查錯誤，本節(jié)我們將通過編寫可執(zhí)行文件的方式來演示獲取和設(shè)置參數(shù)的不同方法。ROS中有如下3種獲取參數(shù)的方式。ros::param::get()ros::NodeHandle::getParam()ros::NodeHandle::param()需要注意，第三種ros::NodeHandle::param()方式在獲取失敗時會自動設(shè)置一個默認(rèn)參數(shù)值。設(shè)置參數(shù)的方式主要有以下兩種。ros::param::set()ros::NodeHandle::setParam()創(chuàng)建文件book_param.cpp，內(nèi)容如下：#include"ros/ros.h"

#include<cstdlib>

usingnamespacestd;

intmain(intargc,char**argv)

{

//initialandnamenode

ros::init(argc,argv,"node_param");

if(argc!=2)

{

cout<<"Errorcommandparamter!Pleaseruncommandeg:\n"\

<<"rosrunbook_parambook_param1\n"\

<<"helpinformation:\n"\

<<"1setparammode(ros::param::set())\n"\

<<"2setparammode(ros::NodeHandle::setParam())\n"\

<<"3getparammode(ros::param::get())\n"\

<<"4getparammode(ros::NodeHandle::getParam())\n"\

<<"5getparammode(ros::NodeHandle::param())\n"\

<<endl;

return1;

}

//createnodehandle

ros::NodeHandlenh;

//paramvariable

intIntParam;

stringStrParam;

boolisIntParam,isStrParam;

//modeflag

intflag=atoi(argv[1]);

//setorgetparamwithdifferentways

switch(flag)

{

case1:

ROS_INFO("setparammode(ros::param::set()):");

ros::param::set("IntParam",1);

ros::param::set("StrParam","stringdemo");

break;

case2:

ROS_INFO("setparammode(ros::NodeHandle::setParam()):");

nh.setParam("IntParam",1);

nh.setParam("StrParam","stringdemo");

break;

case3:

ROS_INFO("getparammode(ros::param::get()):");

isIntParam=ros::param::get("IntParam",IntParam);

isStrParam=ros::param::get("StrParam",StrParam);

if(isIntParam){

ROS_INFO("TheIntParamis:%d",IntParam);

}else{

ROS_INFO("GetIntParamfail!");

}

if(isIntParam){

ROS_INFO("TheStrParamis:%s",StrParam.c_str());

}else{

ROS_INFO("GetStrParamfail!");

}

break;

case4:

ROS_INFO("getparammode(ros::NodeHandle::getParam()):");

isIntParam=nh.getParam("IntParam",IntParam);

isStrParam=nh.getParam("StrParam",StrParam);

if(isIntParam){

ROS_INFO("TheIntParamis:%d",IntParam);

}else{

ROS_INFO("GetIntParamfail!");

}

if(isIntParam){

ROS_INFO("TheStrParamis:%s",StrParam.c_str());

}else{

ROS_INFO("GetStrParamfail!");

}

break;

case5:

ROS_INFO("getparammode(ros::NodeHandle::param()):");

//warning:thiswaywillsetdefaultvaluewhengetnoparam!

nh.param("IntParam",IntParam,11);

//becarefulwhenuseros::NodeHandle::paramgetstringparam!

nh.param<std::string>("StrParam",StrParam,"stringdemo_default");

ROS_INFO("\nTheIntParamis:%d\nTheStrParamis:%s",IntParam,StrParam.c_str());

break;

default:

ROS_INFO("flagvalueisnotinrange:[1,5]");

}

return0;

}

說明如下。if(argc!=2)：如果用戶輸入命令行錯誤，則打印提示信息，有5種模式展示如何獲取和設(shè)置參數(shù)。intIntParam;

stringStrParam;

分別用于設(shè)置int和string兩種類型的參數(shù)。intflag=atoi(argv[1])：模式選擇標(biāo)志位。case1：ros::param::set()方法設(shè)置IntParam和StrParam參數(shù)。case2：ros::NodeHandle::setParam()方法設(shè)置IntParam和StrParam參數(shù)。case3：ros::param::get()方法獲取IntParam和StrParam參數(shù)。case4：ros::NodeHandle::getParam()方法獲取IntParam和StrParam參數(shù)。case5：ros::NodeHandle::param()方法獲取IntParam和StrParam參數(shù)。2.5.2CMakeLists.txt文件創(chuàng)建CMakeLists.txt文件，內(nèi)容如下：cmake_minimum_required(VERSION2.8.3)

project(book_param)

find_package(catkinREQUIREDCOMPONENTSroscpprospystd_msgs)

catkin_package(

)

include_directories(

include${catkin_INCLUDE_DIRS}

)

add_executable(book_param

src/book_param.cpp

)

add_dependencies(book_param${${PROJECT_NAME}_EXPORTED_TARGETS}${catkin_EXPORTED_TARGETS})

target_link_libraries(book_param

${catkin_LIBRARIES}

)

編譯后獲取book_param可執(zhí)行文件。2.5.3測試打開終端，運(yùn)行：rosrunbook_parambook_param

打印錯誤提示信息如下：Errorcommandparamter!Pleaseruncommandeg:

rosrunbook_parambook_param

helpinformation:

1setparammode(ros::param::set())

2setparammode(ros::NodeHandle::setParam())

3getparammode(ros::param::get())

4getparammode(ros::NodeHandle::getParam())

5getparammode(ros::NodeHandle::param())

若運(yùn)行參數(shù)不在設(shè)定范圍內(nèi)，如：rosrunbook_parambook_param6

則打印出的錯誤提示信息如下：[INFO][1551689308.305949069]:flagvalueisnotinrange:[1,5]

重新運(yùn)行：rosrunbook_parambook_param1

打印提示信息：[INFO][1551688969.788377908]:setparammode(ros::param::set()):

查看參數(shù)列表：rosparamlist

打印出所有參數(shù)：/IntParam

/StrParam

命令行獲取參數(shù)值：rosparamgetStrParam

打印出參數(shù)值：Stringdemo

你也可以分別運(yùn)行rosrunbook_parambook_param2(3,4,5)來測試不同模式設(shè)置和獲取參數(shù)的效果。2.6動態(tài)參數(shù)設(shè)置通常，調(diào)試時（尤其是在導(dǎo)航及建圖應(yīng)用中）需要經(jīng)常修改程序中的參數(shù)值，這時無論是修改命令行，還是編寫固定修改參數(shù)的可執(zhí)行文件，都無法滿足要求。ROS為我們提供了動態(tài)參數(shù)設(shè)置的機(jī)制，接下來我們將練習(xí)編寫具備動態(tài)參數(shù)設(shè)置功能的可執(zhí)行文件。2.6.1創(chuàng)建cfg文件創(chuàng)建動態(tài)參數(shù)ROS包book_dynamic_param，加入依賴項roscpp,rospy,dynamic_reconfigure。在功能包下新建cfg文件夾，并創(chuàng)建DynamicParam.cfg文件，內(nèi)容如下：#!/usr/bin/envpython

PACKAGE="book_dynamic_param"

fromdynamic_reconfigure.parameter_generator_catkinimport*

gen=ParameterGenerator()

gen.add("IntDyParam",int_t,0,"AnIntParameter",0,0,9)

gen.add("DouDyParam",double_t,0,"ADoubleParameter",1.5,0,9)

gen.add("StrDyParam",str_t,0,"AStringParameter","Hello,I'mRobot!")

gen.add("BoolDyParam",bool_t,0,"ABoolParameter",True)

student_info=gen.enum([gen.const("Name",str_t,"LiLei","NameInformation"),

gen.const("Sex",str_t,"Man","SexInformation"),

gen.const("Age",str_t,"18","AgeInformation")],

"Asetcontainastudentinformation")

gen.add("StudentInfo",str_t,0,"Astudenetinformationset","LiLei",edit_method=student_info)

exit(gen.generate(PACKAGE,"node_DynamicParam","DynamicParam"))

該配置文件使用Python語言實現(xiàn)，首先需要導(dǎo)入dynamic_reconfigure功能包提供的參數(shù)生成器，通過gen=ParameterGenerator()創(chuàng)建生成器。這里定義了4個不同類型的參數(shù)：int_t、double_t、str_t、bool_t。使用參數(shù)生成器的add(name,type,level,description,default,min,max)方法生成參數(shù)，具體用法如下。name：參數(shù)名，使用字符串描述。type：定義參數(shù)的類型，可以是int_t、double_t、str_t或者bool_t。level：需要傳入?yún)?shù)動態(tài)配置回調(diào)函數(shù)中的掩碼，在回調(diào)函數(shù)中會修改所有參數(shù)的掩碼，表示參數(shù)已經(jīng)進(jìn)行修改。description：描述參數(shù)作用的字符串。default：設(shè)置參數(shù)的默認(rèn)值。min：可選，設(shè)置參數(shù)的最小值，對于字符串和布爾類型值不生效。max：可選，設(shè)置參數(shù)的最大值，對于字符串和布爾類型值不生效。然后利用gen.enum方法生成一個枚舉類型的值，最