機(jī)器人操作系統(tǒng)（ROS）課件6.ROS常用工具

機(jī)器人操作系統(tǒng)（ROS）機(jī)器人操作系統(tǒng)（ROS）ROS常用工具5機(jī)器人操作系統(tǒng)（ROS）5.ROS常用工具ROS為機(jī)器人開發(fā)提供大量實(shí)用的組件工具。本章我們來學(xué)習(xí)ROS中的幾種常用工具，包括Qt工具箱、rviz可視化工具、gazebo仿真環(huán)境、rosbag數(shù)據(jù)記錄與回放和TF工具。Qt工具箱：提供多種機(jī)器人開發(fā)的可視化工具，如日志輸出、計(jì)算圖可視化、數(shù)據(jù)繪圖等功能。rviz三維可視化平臺(tái)：實(shí)現(xiàn)機(jī)器人開發(fā)過程中多種數(shù)據(jù)的可視化顯示。gazebo仿真環(huán)境：創(chuàng)建仿真環(huán)境并實(shí)現(xiàn)帶有物理屬性的機(jī)器人仿真。rosbag數(shù)據(jù)記錄與回放：記錄并回放ROS系統(tǒng)中運(yùn)行時(shí)的所有話題信息，方便后期調(diào)試使用。TF工具：管理機(jī)器人系統(tǒng)中坐標(biāo)系變換關(guān)系。機(jī)器人操作系統(tǒng)（ROS）6.1Qt工具箱Qt工具箱提供多種機(jī)器人開發(fā)的可視化工具，如日志輸出、計(jì)算圖可視化、數(shù)據(jù)繪圖、參數(shù)動(dòng)態(tài)配置等功能。ROS提供了一個(gè)Qt架構(gòu)的后臺(tái)圖形工具套件——rqt_common_plugins，其中包含不少實(shí)用的工具，可通過以下命令進(jìn)行安裝：$sudoapt-getinstallros-melodic-rqt$sudoapt-getinstallros-melodic-rqt-common-plugins機(jī)器人操作系統(tǒng)（ROS）6.1.1日志輸出工具（rqt_console）日志輸出工具用于輸出日志內(nèi)容（Message）、日志級(jí)別（Severity）、節(jié)點(diǎn)（Node）、時(shí)間戳（Stamp）、話題（Topics）、位置（Location）等信息。使用以下命令即可啟動(dòng)該工具：$roscore$rqt_console啟動(dòng)成功后可以看到如圖6-1所示的可視化界面。當(dāng)系統(tǒng)中有不同級(jí)別的日志消息時(shí)，rqt_console的界面中就會(huì)依次顯示這些日志的相關(guān)內(nèi)容，包括日志內(nèi)容、時(shí)間戳、級(jí)別等。當(dāng)日志較多時(shí)，也可以使用該工具進(jìn)行過濾顯示。日志輸出工具界面機(jī)器人操作系統(tǒng)（ROS）6.1.2計(jì)算圖可視化工具（rqt_graph）計(jì)算圖是ROS處理數(shù)據(jù)的一種點(diǎn)對(duì)點(diǎn)的網(wǎng)絡(luò)形式。程序運(yùn)行時(shí)，所有進(jìn)程及它們所進(jìn)行的數(shù)據(jù)處理，將會(huì)通過一種點(diǎn)對(duì)點(diǎn)的網(wǎng)絡(luò)形式表現(xiàn)出來，即通過節(jié)點(diǎn)、節(jié)點(diǎn)管理器、話題、服務(wù)等來進(jìn)行表現(xiàn)。在系統(tǒng)運(yùn)行時(shí)，使用如下命令即可啟動(dòng)該工具：$rqt_graph啟動(dòng)成功后的計(jì)算圖顯示如圖所示。rqt_graph工具界面機(jī)器人操作系統(tǒng)（ROS）6.1.3數(shù)據(jù)繪圖工具（rqt_plot）rqt_plot是一個(gè)二維數(shù)值曲線繪制工具，可以將需要顯示的數(shù)據(jù)在xy坐標(biāo)系中使用曲線描繪，用于觀察變量隨時(shí)間的變化趨勢曲線。使用如下命令即可啟動(dòng)該工具：$rqt_plot然后在界面上方的Topic輸入框中輸入需要顯示的話題消息，如果不確定話題名稱，可以在終端中使用“rostopiclist”命令查看。例如在烏龜例程中，通過rqt_plot工具描繪烏龜x、y坐標(biāo)變化的效果如圖6-3所示。rqt_plot工具機(jī)器人操作系統(tǒng)（ROS）6.1.4參數(shù)動(dòng)態(tài)配置工具（rqt_reconfigure）rqt_reconfigure工具可以在不重啟系統(tǒng)的情況下，動(dòng)態(tài)配置ROS系統(tǒng)中的參數(shù)，但是該功能的使用需要在代碼中設(shè)置參數(shù)的相關(guān)屬性，從而支持動(dòng)態(tài)配置。使用如下命令即可啟動(dòng)該工具：$rosrunrqt_reconfigurerqt_reconfigure啟動(dòng)后的界面將顯示當(dāng)前系統(tǒng)中所有可動(dòng)態(tài)配置的參數(shù)如圖6-4所示，在界面中使用輸入框、滑動(dòng)條或下拉框進(jìn)行設(shè)置即可實(shí)現(xiàn)參數(shù)的動(dòng)態(tài)配置。rqt_reconfigure工具界面機(jī)器人操作系統(tǒng)（ROS）6.2rviz三維可視化平臺(tái)ROS針對(duì)機(jī)器人系統(tǒng)的可視化需求，為用戶提供了一款顯示多種數(shù)據(jù)的三維可視化平臺(tái)rviz。rviz是ROS中一款三維可視化平臺(tái)，一方面能夠?qū)崿F(xiàn)對(duì)外部信息的圖形化顯示，另外還可以通過rviz發(fā)布控制信息，從而實(shí)現(xiàn)對(duì)機(jī)器人的監(jiān)測與控制。rviz可以幫助開發(fā)者實(shí)現(xiàn)所有可監(jiān)測信息的圖形化顯示，開發(fā)者也可以在rviz的控制界面下，通過按鈕、滑動(dòng)條、數(shù)值等方式控制機(jī)器人的行為。機(jī)器人操作系統(tǒng)（ROS）6.2.1運(yùn)行rvizrviz已經(jīng)集成在桌面完整版的ROS中，如果已經(jīng)成功安裝桌面完整版的ROS，可以直接跳過這一步驟，否則，請(qǐng)使用如下命令進(jìn)行安裝：$sudoapt-getinstallros-melodic-rviz安裝完成后，在終端中分別運(yùn)行如下命令即可啟動(dòng)ROS和rviz平臺(tái)：$roscore$rosrunrvizrviz啟動(dòng)成功的rviz主界面如圖所示。rviz的主界面機(jī)器人操作系統(tǒng)（ROS）6.2.1運(yùn)行rvizDisplays面板位于rviz的左側(cè)，包含rviz的顯示插件列表及其屬性，可以通過點(diǎn)擊左側(cè)的Add按鈕來添加所需的插件，也可以編寫自己的顯示插件并將其添加到面板上。3D視圖顯示區(qū)位于rviz的中間部分，用于可視化不同類型的ROS消息，例如激光雷達(dá)3D點(diǎn)云、地圖信息、里程計(jì)數(shù)據(jù)、機(jī)器人模型、tf等。工具欄位于rviz的頂部，這些工具可以用來與機(jī)器人模型交互、調(diào)整相機(jī)視圖、設(shè)置導(dǎo)航目標(biāo)以及設(shè)置機(jī)器人2D位姿估計(jì)等，同時(shí)還可以在工具欄中以插件的形式添加自定義的工具。Views顯示面板位于rviz的右側(cè)，可以保存3D視角的不同視圖，也可以通過加載已保存的配置來切換到相應(yīng)的視圖模式下。Time時(shí)間顯示面板位于rviz的底部，展示了仿真花費(fèi)的時(shí)間。可以通過點(diǎn)擊面板上的Reset按鈕重置rviz的初始設(shè)置。機(jī)器人操作系統(tǒng)（ROS）6.2.2數(shù)據(jù)可視化為了解釋數(shù)據(jù)可視化，我們將用到turtlesim的示例，運(yùn)行程序：$roslaunchturtle_tfturtle_tf_demo.launch這是tf坐標(biāo)變換的示例程序，通過鍵盤控制小海龜1，小海龜2會(huì)跟隨其運(yùn)動(dòng),效果如圖所示。接下來我們可以通過rviz工具查看其運(yùn)動(dòng)的變化。機(jī)器人操作系統(tǒng)（ROS）6.2.2數(shù)據(jù)可視化首先，在Display面板里的全局選項(xiàng)，更改FixedFrame為/world。然后將TF樹添加到Display面板，點(diǎn)擊rviz界面左側(cè)下方的“Add”按鈕，rviz會(huì)將默認(rèn)支持的所有數(shù)據(jù)類型的顯示插件羅列出來，如圖所示。機(jī)器人操作系統(tǒng)（ROS）6.2.2數(shù)據(jù)可視化在上一張圖所示的列表中選擇需要的TF，然后在“DisplayName”文本框中填入一個(gè)唯一的名稱tf_turtle，用來識(shí)別顯示的數(shù)據(jù)。添加完成后，rviz左側(cè)的Dispalys中會(huì)列出已經(jīng)添加的顯示插件；點(diǎn)擊插件列表前的加號(hào)，可以打開一個(gè)屬性列表，根據(jù)需求設(shè)置屬性。如果訂閱成功，在中間的顯示區(qū)應(yīng)該會(huì)出現(xiàn)可視化后的數(shù)據(jù)，結(jié)果如圖所示。機(jī)器人操作系統(tǒng)（ROS）6.3Gazebo仿真環(huán)境Gazebo是一款3D動(dòng)態(tài)模擬器，能夠在復(fù)雜的室內(nèi)和室外環(huán)境中準(zhǔn)確有效地模擬機(jī)器人。與游戲引擎提供高保真度的視覺模擬類似，Gazebo提供高保真度的物理模擬，其提供一整套傳感器模型，以及對(duì)用戶和程序非常友好的交互方式。雖然Gazebo中的機(jī)器人模型與rviz使用的模型相同，但是需要在模型中加入機(jī)器人和周圍環(huán)境的物理屬性，例如質(zhì)量、摩擦系數(shù)、彈性系數(shù)等。機(jī)器人的傳感器信息也可以通過插件的形式加入仿真環(huán)境，以可視化的方式進(jìn)行顯示。機(jī)器人操作系統(tǒng)（ROS）6.3.1安裝并運(yùn)行Gazebo與rviz一樣，如果已經(jīng)安裝了桌面完整版的ROS，那么可以直接跳過這一步，否則，可以使用以下命令進(jìn)行安裝：$sudoapt-getinstallros-melodic-gazebo-ros-pkgsros-melodic-gazebo-ros-control安裝完成后，在終端中使用如下命令啟動(dòng)ROS和Gazebo：$roscore$rosrungazebo_rosgazeboGazebo啟動(dòng)成功后的界面如圖所示。機(jī)器人操作系統(tǒng)（ROS）6.3.1安裝并運(yùn)行Gazebo主界面中主要包含3D視圖區(qū)、工具欄、模型列表、模型屬性項(xiàng)和時(shí)間顯示區(qū)。驗(yàn)證Gazebo是否與ROS系統(tǒng)成功連接，可以查看ROS的話題列表：$rostopiclist如果連接成功，應(yīng)該可以看到Gazebo發(fā)布/訂閱的話題，如圖所示。機(jī)器人操作系統(tǒng)（ROS）6.3.1安裝并運(yùn)行Gazebo當(dāng)然，還有Gazebo提供的服務(wù)列表，如圖所示，使用以下命令查看：$rosservicelist機(jī)器人操作系統(tǒng)（ROS）6.3.2構(gòu)建仿真環(huán)境Gazebo中有兩種創(chuàng)建仿真環(huán)境的方法，包括直接插入模型和BuildingEditor。（1）直接插入模型首先需要下載相關(guān)模型庫到本地路徑~/.gazebo下，模型下載地址：/osrf/gazebo_models，使用如下命令：$cd~/.gazebo$gitclone/osrf/gazebo_models.git在Gazebo左側(cè)的模型列表中，有一個(gè)insert選項(xiàng)羅列了所有可使用的模型。選擇需要使用的模型，放置在主顯示區(qū)中，如圖所示，就可以在仿真環(huán)境中添加機(jī)器人和外部物體等仿真實(shí)例。機(jī)器人操作系統(tǒng)（ROS）6.3.2構(gòu)建仿真環(huán)境（2）BuildingEditor第二種方法是使用Gazebo提供的BuildingEditor工具手動(dòng)繪制地圖。在Gazebo菜單欄中選擇Edit→BuildingEditor，可以打開如圖6-13所示的BuildingEditor界面。選擇左側(cè)的繪制選項(xiàng)，然后在上側(cè)窗口中使用鼠標(biāo)繪制，下側(cè)窗口中即可實(shí)時(shí)顯示繪制的仿真環(huán)境。機(jī)器人操作系統(tǒng)（ROS）6.4rosbag數(shù)據(jù)記錄與回放ROS能夠存儲(chǔ)所有節(jié)點(diǎn)通過話題發(fā)布的消息。ROS提供了數(shù)據(jù)記錄與回放的功能包rosbag，包含消息所有字段參數(shù)和時(shí)間戳，可以幫助開發(fā)者收集ROS系統(tǒng)運(yùn)行時(shí)的消息數(shù)據(jù)，然后在離線狀態(tài)下進(jìn)行回放和處理。本節(jié)將通過小海龜例程介紹rosbag數(shù)據(jù)記錄和回放的實(shí)現(xiàn)方法。機(jī)器人操作系統(tǒng)（ROS）6.4.1使用rosbag首先啟動(dòng)鍵盤控制小海例程所需的所有節(jié)點(diǎn)：$roscore$rosrunturtlesimturtlesim_node$rosrunturtlesimturtle_teleop_key啟動(dòng)成功后，可以看到可視化界面中的小烏龜了，此時(shí)可以在終端中通過鍵盤控制小烏龜移動(dòng)。然后我們使用以下命令查看在當(dāng)前ROS系統(tǒng)中存在的話題列表，結(jié)果如圖6-14所示：$rostopiclist-v機(jī)器人操作系統(tǒng)（ROS）6.4.1使用rosbag接下來使用rosbag抓取這些話題的消息，并且打包成一個(gè)文件放置到指定文件夾中：$mkdir~/rosbagfiles$cd~/rosbagfiles$rosbagrecord-a其中，-a(all)參數(shù)表示記錄所有發(fā)布的消息。此時(shí)消息記錄已經(jīng)開始，我們可以在終端中控制小烏龜移動(dòng)一段時(shí)間，然后進(jìn)入剛才創(chuàng)建的文件夾~/bagfiles中，應(yīng)該會(huì)有一個(gè)以時(shí)間命名并且以.bag為后綴的文件。機(jī)器人操作系統(tǒng)（ROS）6.4.2回放數(shù)據(jù)我們已經(jīng)有一個(gè)消息記錄的文件包，之后我們就可以用它回放數(shù)據(jù)。關(guān)閉之前打開的turtle_teleop_key鍵盤控制節(jié)點(diǎn)并重啟turtlesim_node，使用如下命令回放所記錄的話題數(shù)據(jù)：$rosbagplay<yourbagfile>在短暫的等待時(shí)間后，數(shù)據(jù)開始回放，小烏龜?shù)倪\(yùn)動(dòng)軌跡應(yīng)該與之前數(shù)據(jù)記錄過程中的狀態(tài)完全相同，在終端中也可以看到如圖所示的回放時(shí)間信息，運(yùn)行結(jié)果小海龜運(yùn)動(dòng)軌跡對(duì)比如圖所示?；胤艛?shù)據(jù)小海龜軌跡對(duì)比圖機(jī)器人操作系統(tǒng)（ROS）6.4.2回放數(shù)據(jù)rosbag功能包還提供了info命令，可以查看數(shù)據(jù)記錄文件的詳細(xì)信息，命令的使用格式如下：$rosbaginfo<yourbagfile>使用info命令來查看之前生成的數(shù)據(jù)記錄文件，可以看到類似圖所示的信息。查看數(shù)據(jù)記錄文件的相關(guān)信息從以上信息中我們可以看到，數(shù)據(jù)記錄包中包含的所有話題、消息類型、消息數(shù)量等信息。機(jī)器人操作系統(tǒng)（ROS）6.5TF工具坐標(biāo)變換作為機(jī)器人學(xué)的基礎(chǔ)，也是機(jī)器人運(yùn)動(dòng)控制不可或缺的部分，掌握機(jī)器人坐標(biāo)變換十分必要。在機(jī)器人開發(fā)中，無論是相機(jī)坐標(biāo)與機(jī)器人坐標(biāo)之間還是世界坐標(biāo)與機(jī)器人坐標(biāo)間，都需要用坐標(biāo)變換統(tǒng)一坐標(biāo)系。ROS為開發(fā)者提供了TF庫，可以很方便地實(shí)現(xiàn)系統(tǒng)中任一個(gè)點(diǎn)在所有坐標(biāo)系之間的坐標(biāo)變換。本節(jié)首先介紹機(jī)器人運(yùn)動(dòng)學(xué)相關(guān)基礎(chǔ)知識(shí)，再介紹TF功能包相關(guān)知識(shí)及其使用方法。機(jī)器人操作系統(tǒng)（ROS）6.5.1機(jī)器人運(yùn)動(dòng)學(xué)基本原理（1）空間坐標(biāo)系的描述坐標(biāo)變換是機(jī)器人運(yùn)動(dòng)學(xué)中的基礎(chǔ)概念。機(jī)器人作為一個(gè)完整的系統(tǒng)，結(jié)構(gòu)上往往由大量組件元素組成，例如機(jī)器人的主體部分、連桿、活動(dòng)關(guān)節(jié)等。在進(jìn)行機(jī)器人設(shè)計(jì)和應(yīng)用的過程中涉及各個(gè)組件位姿的變化，機(jī)器人的構(gòu)型通常根據(jù)他們的坐標(biāo)系來確定，這就引入了空間坐標(biāo)系的概念。在機(jī)器人運(yùn)動(dòng)學(xué)中，使用矩陣來表示坐標(biāo)系和坐標(biāo)系中的物體。我們用x、y和z軸表示固定的全局參考坐標(biāo)系F_(x,y,z),用n、o和a軸表示相對(duì)于參考坐標(biāo)系的另一個(gè)運(yùn)動(dòng)坐標(biāo)系F_(n,o,a)。字母n、o和a取自于單詞normal、orientation和approach的首字母。參考圖6-18,很顯然為了避免在抓取物體時(shí)發(fā)生碰撞，機(jī)器人必須沿著抓手的z軸方向來接近該物體。用機(jī)器人的術(shù)語，這個(gè)軸稱為接近(approach)軸，簡稱為a軸。在抓手坐標(biāo)系中接近物體的方向稱為方向(orientation)軸，簡稱為o軸。又因?yàn)閤軸垂直(normal)于上述兩軸，所以簡稱為n軸。因此，在本節(jié)中均稱由normal、orientation和approach軸組成的運(yùn)動(dòng)坐標(biāo)系為F_(n,o,a)。機(jī)器人操作系統(tǒng)（ROS）6.5.1機(jī)器人運(yùn)動(dòng)學(xué)基本原理（2）空間坐標(biāo)系的轉(zhuǎn)換空間坐標(biāo)系的轉(zhuǎn)換可以分為三類：純平移、繞一個(gè)軸純旋轉(zhuǎn)、平移和旋轉(zhuǎn)相結(jié)合。①純平移如果坐標(biāo)系在空間以不變的姿態(tài)運(yùn)動(dòng)，那么該變換就是純平移變換。在這種情況下，他的方向單位向量保持統(tǒng)一方向不變。所有的改變只是坐標(biāo)系原點(diǎn)相對(duì)于參考坐標(biāo)系的變化。機(jī)器人操作系統(tǒng)（ROS）6.5.1機(jī)器人運(yùn)動(dòng)學(xué)基本原理

機(jī)器人操作系統(tǒng)（ROS）6.5.1機(jī)器人運(yùn)動(dòng)學(xué)基本原理

機(jī)器人操作系統(tǒng)（ROS）6.5.1機(jī)器人運(yùn)動(dòng)學(xué)基本原理

機(jī)器人操作系統(tǒng)（ROS）6.5.1機(jī)器人運(yùn)動(dòng)學(xué)基本原理

機(jī)器人操作系統(tǒng)（ROS）6.5.1機(jī)器人運(yùn)動(dòng)學(xué)基本原理

機(jī)器人操作系統(tǒng)（ROS）6.5.2TF功能包的原理分析（/tf#tf_echo）TF是一個(gè)讓用戶隨時(shí)間跟蹤多個(gè)坐標(biāo)系的功能包，它維護(hù)多個(gè)坐標(biāo)系之間的坐標(biāo)關(guān)系，可以幫助開發(fā)者在任意時(shí)間，在坐標(biāo)系間完成點(diǎn)、向量等坐標(biāo)的變換。機(jī)器人操作系統(tǒng)（ROS）6.5.2TF功能包的原理分析（/tf#tf_echo）如圖6-22所示，一個(gè)機(jī)器人系統(tǒng)通常由很多三維坐標(biāo)系，而且會(huì)隨時(shí)間的推移發(fā)生變化，如世界坐標(biāo)系（WorldFrame）、基坐標(biāo)系（BaseFrame）、機(jī)械夾爪坐標(biāo)系（GripperFrame）、機(jī)器人頭部坐標(biāo)系（HeadFrame）等。TF以時(shí)間為軸跟蹤這些坐標(biāo)系，并且允許開發(fā)者請(qǐng)求如下類型的數(shù)據(jù)：(1)機(jī)器人夾取的物體相對(duì)于機(jī)器人頭部的位置在哪里？(2)機(jī)器人頭部坐標(biāo)系相對(duì)于中心坐標(biāo)系的關(guān)系是什么樣的？(3)機(jī)器人中心坐標(biāo)系相對(duì)于激光雷達(dá)傳感器的位置在哪里？TF可以在分布式系統(tǒng)中進(jìn)行操作，也就是說，一個(gè)機(jī)器人系統(tǒng)中所有的坐標(biāo)變換關(guān)系，對(duì)于所有的節(jié)點(diǎn)組件都是可用的，所有訂閱TF消息的節(jié)點(diǎn)都會(huì)緩沖一份所有坐標(biāo)系的變換關(guān)系數(shù)據(jù)，所以這種結(jié)構(gòu)不需要中心服務(wù)器來存儲(chǔ)任何數(shù)據(jù)。想要使用TF功能包，總體來說需要以下兩個(gè)步驟。(1)監(jiān)聽TF變換：接收并緩存系統(tǒng)中發(fā)布的所有坐標(biāo)變換數(shù)據(jù)，并從中查詢所需要的坐標(biāo)變換關(guān)系。(2)廣播TF變換：向系統(tǒng)中廣播坐標(biāo)系之間的坐標(biāo)變換關(guān)系。系統(tǒng)中可能會(huì)存在多個(gè)不同部分的TF變換廣播，每個(gè)廣播都可以直接將坐標(biāo)變換關(guān)系插入TF樹中，不需要再進(jìn)行同步。機(jī)器人操作系統(tǒng)（ROS）6.5.3TF工具TF提供了豐富的終端工具來幫助開發(fā)者調(diào)試和創(chuàng)建TF變換。如果已經(jīng)安裝了桌面完整版的ROS，那么可以不用單獨(dú)安裝TF工具，否則，可以使用以下命令進(jìn)行安裝：$sudoapt-getinstallros-melodic-tf(1) tf_monitor:查看指定坐標(biāo)系之間的發(fā)布狀態(tài)，命令格式如下：$rosruntftf_monitor<source_frame><target_target>(2) tf_echo:查看指定坐標(biāo)系之間的變換關(guān)系，命令格式如下：$rosruntftf_echo<source_frame><target_frame>機(jī)器人操作系統(tǒng)（ROS）6.5.3TF工具(3) view_frame:可視化調(diào)試工具，生成pdf文件，顯示整個(gè)TF數(shù)，命令格式如下：$rosruntfview_frames(4) static_transform_publisher:發(fā)布兩個(gè)坐標(biāo)系之間的靜態(tài)坐標(biāo)變換，這兩個(gè)坐標(biāo)系不發(fā)生相對(duì)位置變化，命令格式如下：$rosruntfstatic_transform_publisherxyzyawpitchrollframe_idchild_frame_idperiod_in_ms$rosruntfstatic_transform_publisherxyzqxqyqzqwframe_idchild_frame_idperiod_in_ms機(jī)器人操作系統(tǒng)（ROS）6.5.4機(jī)器人的ROStf應(yīng)用實(shí)例本節(jié)將通過一個(gè)烏龜例程來簡要介紹ROS中TF的作用，學(xué)習(xí)常用的TF工具。首先使用如下命令安裝Ubuntu發(fā)布的turtle_tf功能包。$sudoapt-getinstallros-melodic-turtle-tf機(jī)器人操作系統(tǒng)（ROS）6.5.4機(jī)器人的ROStf應(yīng)用實(shí)例現(xiàn)在我們已經(jīng)完成了turtle_tf功能包的獲取，接下來就可以使用如下命令運(yùn)行例程了：$roslaunchturtle_tfturtle_tf_demo.launch機(jī)器人操作系統(tǒng)（ROS）6.5.4機(jī)器人的ROStf應(yīng)用實(shí)例仿真器中將出現(xiàn)小烏龜，說明turtle_tf功能包的啟動(dòng)文件已經(jīng)成功打開，并且處于下方的小烏龜會(huì)自動(dòng)向中心位置的小烏龜靠攏。打開一個(gè)新的終端，打開鍵盤控制節(jié)點(diǎn)，從而可以使用鍵盤上的上下左右鍵控制中心位置的小烏龜運(yùn)動(dòng)。$rosrunturtlesimturtle_teleop_key上述代碼運(yùn)行了turtlesim功能包下的turtle_teleop_key鍵盤控制節(jié)點(diǎn)，通過鍵盤控制可以發(fā)現(xiàn)另一只烏龜總是跟隨我們控制的那只烏龜運(yùn)行。機(jī)器人操作系統(tǒng)（ROS）6.5.4機(jī)器人的ROStf應(yīng)用實(shí)例通過TF工具view_frames可以查看當(dāng)前例程中的TF樹。$rosruntfview_frames機(jī)器人操作系統(tǒng)（ROS）6.5.4機(jī)器人的ROStf應(yīng)用實(shí)例在這里我們可以看到tf廣播的三個(gè)坐標(biāo)系：世界坐標(biāo)系、海龜1坐標(biāo)系和海龜2坐標(biāo)系。我們還可以看到world是turtle1和turtle2坐標(biāo)系的父級(jí)。出于調(diào)試目的，view_frames還會(huì)報(bào)告一些有關(guān)何時(shí)接收到最早和最近的坐標(biāo)轉(zhuǎn)換以及tf坐標(biāo)發(fā)布到tf.frame的速度診斷信息。通過tf_echo工具查看兩個(gè)烏龜坐標(biāo)系的變換關(guān)系，命令如下：$rosruntftf_echoturtle1turtle2機(jī)器人操作系統(tǒng)（ROS）6.5.4機(jī)器人的ROStf應(yīng)用實(shí)例其中打印結(jié)果中Translation表示位置信息，Rotation表示角度信息。當(dāng)控制海龜四處走動(dòng)時(shí)，就可以看到隨著兩只海龜相對(duì)移動(dòng)而發(fā)生的坐標(biāo)變化。通過rviz圖形界面可以更加形象地看到這三者之間的坐標(biāo)關(guān)系。$rosrunrvizrviz-d`rospackfindturtle_tf`/rviz/turtle_rviz.rviz機(jī)器人操作系統(tǒng)（ROS）6.5.4機(jī)器人的ROStf應(yīng)用實(shí)例

機(jī)器人操作系統(tǒng)（ROS）6.5.4機(jī)器人的ROStf應(yīng)用實(shí)例（1）TF的廣播TF能在ROS中建立坐標(biāo)系，并且能夠簡化各個(gè)坐標(biāo)系之間的轉(zhuǎn)換關(guān)系。可以使用C++或者Python來編寫tf廣播器。我們需要?jiǎng)?chuàng)建一個(gè)發(fā)布烏龜坐標(biāo)系與世界坐標(biāo)系之間TF變換的節(jié)點(diǎn)，實(shí)現(xiàn)源碼src/turtle_tf_broadcaster.cpp的具體內(nèi)容如下：#include<ros/ros.h>#include<tf/transform_broadcaster.h>#include<turtlesim/Pose.h>std::stringturtle_name;voidposeCallback(constturtlesim::PoseConstPtr&msg){//tf廣播器statictf::TransformBroadcasterbr;//根據(jù)烏龜當(dāng)前的位姿，設(shè)置相對(duì)于世界坐標(biāo)系的坐標(biāo)變換tf::Transformtransform;transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));tf::Quaternionq;q.setRPY(0,0,msg->theta);transform.setRotation(q);//發(fā)布坐標(biāo)變換br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));}intmain(intargc,char**argv){//初始化節(jié)點(diǎn)ros::init(argc,argv,"my_tf_broadcaster");if(argc!=2){ROS_ERROR("needturtlenameasargument");return-1;};turtle_name=argv[1];//訂閱烏龜?shù)膒ose信息ros::NodeHandlenode;ros::Subscribersub=node.subscribe(turtle_name+"/pose",10,&poseCallback);ros::spin();return0;};機(jī)器人操作系統(tǒng)（ROS）6.5.4機(jī)器人的ROStf應(yīng)用實(shí)例下面簡單剖析以上代碼的實(shí)現(xiàn)過程。①頭文件部分#include<ros/ros.h>#include<tf/transform_broadcaster.h>#include<turtlesim/Pose.h>為了避免包含繁雜的ROS功能包頭文件，ros/ros.h已經(jīng)幫我們包含了大部分ROS中通用的頭文件。要使用TransformBroadcaster，我們需要包含tf/transform_broadcaster.h頭文件。機(jī)器人操作系統(tǒng)（ROS）6.5.4機(jī)器人的ROStf應(yīng)用實(shí)例②poseCallback函數(shù)部分statictf::TransformBroadcasterbr;tf::Transformtransform;定義一個(gè)廣播，相當(dāng)于發(fā)布話題時(shí)定義一個(gè)發(fā)布器。定義存放轉(zhuǎn)換信息（平移，旋轉(zhuǎn)）的變量。transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));setOrigin()函數(shù)設(shè)置坐標(biāo)原點(diǎn)，需要注意該函數(shù)的類型需要為tf::Vector3類型，假設(shè)是要發(fā)布一個(gè)子坐標(biāo)系為“turtle1”父坐標(biāo)系為“world”，那么其中(msg->x,msg->y,0.0)是指“turtle1”的坐標(biāo)原點(diǎn)在“world”坐標(biāo)系下的坐標(biāo)。tf::Quaternionq;q.setRPY(0,0,msg->theta);transform.setRotation(q);此部分定義了旋轉(zhuǎn)，需要注意setRPY()函數(shù)的參數(shù)為”turtle1”在“world”坐標(biāo)系下的roll(繞X軸)，pitch(繞Y軸)，yaw(繞Z軸)。br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));此部分中包含四個(gè)參數(shù)，transform是存儲(chǔ)變換關(guān)系的變量；ros::Time::now()是廣播tf時(shí)的時(shí)間戳；“world”是父坐標(biāo)系的名字；turtle_name是子坐標(biāo)系的名字。機(jī)器人操作系統(tǒng)（ROS）6.5.4機(jī)器人的ROStf應(yīng)用實(shí)例（2）TF的監(jiān)聽TF消息廣播之后，其他節(jié)點(diǎn)就可以監(jiān)聽該TF消息，從而獲取需要的坐標(biāo)變換了。目前我們已經(jīng)將烏龜相對(duì)于world坐標(biāo)系的TF變換進(jìn)行了廣播，接下來需要監(jiān)聽TF消息，并從中獲取turtle2相對(duì)于turtle1坐標(biāo)系的變換，從而控制turtle2移動(dòng)。簡單的來說，通過監(jiān)聽tf，我們可以避免繁瑣的旋轉(zhuǎn)矩陣的計(jì)算，而直接獲取我們需要的相關(guān)信息。實(shí)現(xiàn)源碼src/turtle_tf_listener.cpp的詳細(xì)內(nèi)容如下：#include<ros/ros.h>#include<tf/transform_listener.h>#include<geometry_msgs/Twist.h>#include<turtlesim/Spawn.h>intmain(intargc,char**argv){//初始化節(jié)點(diǎn)ros::init(argc,argv,"my_tf_listener");ros::NodeHandlenode;//通過服務(wù)調(diào)用，產(chǎn)生第二只烏龜turtle2ros::service::waitForService("spawn");ros::ServiceClientadd_turtle=node.serviceClient<turtlesim::Spawn>("spawn");turtlesim::Spawnsrv;add_turtle.call(srv);//定義turtle2的速度控制發(fā)布器ros::Publisherturtle_vel=node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10);//tf監(jiān)聽器tf::TransformListenerlistener;ros::Raterate(10.0);while(node.ok()){tf::StampedTransformtransform;try{//查找turtle2與turtle1的坐標(biāo)變換listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform);}catch(tf::TransformException&ex){ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}//根據(jù)turtle1和turtle2之間的坐標(biāo)變換，計(jì)算turtle2需要運(yùn)動(dòng)的線速度和角速度//并發(fā)布速度控制指令，使turtle2向turtle1移動(dòng)geometry_msgs::Twistvel_msg;vel_msg.angular.z=4.0*atan2(transform.getOrigin().y(),transform.getOrigin().x());vel_msg.linear.x=0.5*sqrt(pow(transform.getOrigin().x(),2)+pow(transform.getOrigin().y(),2));turtle_vel.publish(vel_msg);rate.sleep();}return0;};機(jī)器人操作系統(tǒng)（ROS）6.5.4機(jī)器人的ROStf應(yīng)用實(shí)例①初始化部分tf::TransformListenerlistener;ros::Raterate(10.0);創(chuàng)建一個(gè)tf::TransformListener類型的監(jiān)聽器，創(chuàng)建成功后監(jiān)聽器會(huì)自動(dòng)接收TF樹的消息，并且緩存10秒。②循環(huán)部分tf::StampedTransformtransform;定義存放變換關(guān)系的變量。listener.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform);監(jiān)聽中兩個(gè)常用的函數(shù)transformPoint（）和lookupTransform()，前者可以獲得兩個(gè)坐標(biāo)系之間轉(zhuǎn)換的關(guān)系，包括旋轉(zhuǎn)與平移，后者可以實(shí)現(xiàn)從“/turtle2”到“/turtle1”的轉(zhuǎn)換。catch(tf::TransformException&ex){ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}監(jiān)聽兩個(gè)坐標(biāo)系之間的變換。由于tf會(huì)把監(jiān)聽的內(nèi)容存放到一個(gè)緩存中，然后再讀取相關(guān)的內(nèi)