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

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

機器人操作系統(tǒng)（ROS）6.5.1機器人運動學基本原理

機器人操作系統(tǒng)（ROS）6.5.1機器人運動學基本原理

機器人操作系統(tǒng)（ROS）6.5.1機器人運動學基本原理

機器人操作系統(tǒng)（ROS）6.5.1機器人運動學基本原理

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

機器人操作系統(tǒng)（ROS）6.5.4機器人的ROStf應用實例（1）TF的廣播TF能在ROS中建立坐標系，并且能夠簡化各個坐標系之間的轉(zhuǎn)換關(guān)系?？梢允褂肅++或者Python來編寫tf廣播器。我們需要創(chuàng)建一個發(fā)布烏龜坐標系與世界坐標系之間TF變換的節(jié)點，實現(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ù)烏龜當前的位姿，設置相對于世界坐標系的坐標變換tf::Transformtransform;transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));tf::Quaternionq;q.setRPY(0,0,msg->theta);transform.setRotation(q);//發(fā)布坐標變換br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));}intmain(intargc,char**argv){//初始化節(jié)點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;};機器人操作系統(tǒng)（ROS）6.5.4機器人的ROStf應用實例下面簡單剖析以上代碼的實現(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頭文件。機器人操作系統(tǒng)（ROS）6.5.4機器人的ROStf應用實例②poseCallback函數(shù)部分statictf::TransformBroadcasterbr;tf::Transformtransform;定義一個廣播，相當于發(fā)布話題時定義一個發(fā)布器。定義存放轉(zhuǎn)換信息（平移，旋轉(zhuǎn)）的變量。transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));setOrigin()函數(shù)設置坐標原點，需要注意該函數(shù)的類型需要為tf::Vector3類型，假設是要發(fā)布一個子坐標系為“turtle1”父坐標系為“world”，那么其中(msg->x,msg->y,0.0)是指“turtle1”的坐標原點在“world”坐標系下的坐標。tf::Quaternionq;q.setRPY(0,0,msg->theta);transform.setRotation(q);此部分定義了旋轉(zhuǎn)，需要注意setRPY()函數(shù)的參數(shù)為”turtle1”在“world”坐標系下的roll(繞X軸)，pitch(繞Y軸)，yaw(繞Z軸)。br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));此部分中包含四個參數(shù)，transform是存儲變換關(guān)系的變量；ros::Time::now()是廣播tf時的時間戳；“world”是父坐標系的名字；turtle_name是子坐標系的名字。機器人操作系統(tǒng)（ROS）6.5.4機器人的ROStf應用實例（2）TF的監(jiān)聽TF消息廣播之后，其他節(jié)點就可以監(jiān)聽該TF消息，從而獲取需要的坐標變換了。目前我們已經(jīng)將烏龜相對于world坐標系的TF變換進行了廣播，接下來需要監(jiān)聽TF消息，并從中獲取turtle2相對于turtle1坐標系的變換，從而控制turtle2移動。簡單的來說，通過監(jiān)聽tf，我們可以避免繁瑣的旋轉(zhuǎn)矩陣的計算，而直接獲取我們需要的相關(guān)信息。實現(xiàn)源碼src/turtle_tf_listener.cpp的詳細內(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é)點ros::init(argc,argv,"my_tf_listener");ros::NodeHandlenode;//通過服務調(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的坐標變換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之間的坐標變換，計算turtle2需要運動的線速度和角速度//并發(fā)布速度控制指令，使turtle2向turtle1移動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;};機器人操作系統(tǒng)（ROS）6.5.4機器人的ROStf應用實例①初始化部分tf::TransformListenerlistener;ros::Raterate(10.0);創(chuàng)建一個tf::TransformListener類型的監(jiān)聽器，創(chuàng)建成功后監(jiān)聽器會自動接收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)聽中兩個常用的函數(shù)transformPoint（）和lookupTransform()，前者可以獲得兩個坐標系之間轉(zhuǎn)換的關(guān)系，包括旋轉(zhuǎn)與平移，后者可以實現(xiàn)從“/turtle2”到“/turtle1”的轉(zhuǎn)換。catch(tf::TransformException&ex){ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}監(jiān)聽兩個坐標系之間的變換。由于tf會把監(jiān)聽的內(nèi)容存放到一個緩存中，然后再讀取相關(guān)的內(nèi)