5.6 坐標(biāo)變換實(shí)操
5.6 坐標(biāo)變換實(shí)操
本節(jié)主要介紹如何實(shí)現(xiàn)”烏龜跟隨“以及“烏龜護(hù)航”案例。
5.6.1 烏龜跟隨案例以及分析
1.案例需求
需求：編寫程序?qū)崿F(xiàn)，程序運(yùn)行后會(huì)啟動(dòng) turtlesim_node 節(jié)點(diǎn)，該節(jié)點(diǎn)會(huì)生成一個(gè)窗口，窗口中有一只原生烏龜（turtle1），緊接著再生成一只新的烏龜（turtle2），無論是 turtle1 靜止或是被鍵盤控制運(yùn)動(dòng)時(shí)，turtle2 都會(huì)以 turtle1 為目標(biāo)并向其運(yùn)動(dòng)。
2.案例分析
“烏龜跟隨”案例的核心是如何確定 turtle1 相對 turtle2 的位置，只要該位置確定就可以把其作為目標(biāo)點(diǎn)并控制 turtle2 向其運(yùn)動(dòng)。相對位置的確定可以通過坐標(biāo)變換實(shí)現(xiàn)，大致思路是先分別廣播 turtle1 相對于 world 和 turtle2 相對于 world 的坐標(biāo)系關(guān)系，然后再通過監(jiān)聽坐標(biāo)系關(guān)系進(jìn)一步獲取 turtle1 相對于 turtle2 的坐標(biāo)系關(guān)系。
3.流程簡介
案例實(shí)現(xiàn)主要步驟如下：
編寫程序調(diào)用 /spawn 服務(wù)生成一只新烏龜；
編寫坐標(biāo)變換廣播實(shí)現(xiàn)，通過該實(shí)現(xiàn)可以廣播 turtle1 相對于 world 和 turtle2 相對于 world 的坐標(biāo)系關(guān)系；
編寫坐標(biāo)變換監(jiān)聽實(shí)現(xiàn)，獲取 turtle1 相對于 turtle2 的坐標(biāo)系關(guān)系并生成控制 turtle2 運(yùn)動(dòng)的速度指令；
編寫 launch 文件集成相關(guān)節(jié)點(diǎn)；
編輯配置文件；
編譯；
執(zhí)行。
案例我們會(huì)采用 C++ 和 Python 分別實(shí)現(xiàn)，二者都遵循上述實(shí)現(xiàn)流程。
4.準(zhǔn)備工作
1.新建功能包
終端下進(jìn)入工作空間的src目錄，調(diào)用如下兩條命令分別創(chuàng)建C++功能包和Python功能包。
ros2 pkg create cpp05_exercise --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs turtlesim?
ros2 pkg create py05_exercise --build-type ament_python --dependencies rclpy tf_transformations tf2_ros geometry_msgs turtlesim
2.創(chuàng)建launch目錄
功能包cpp05_exercise和py05_exercise下分別新建launch文件，并編輯配置文件。
功能包cpp05_exercise的 CMakeLists.txt 文件添加如下內(nèi)容：
功能包py05_exercise的 setup.py 文件中需要修改 data_files 屬性，修改后的內(nèi)容如下：
5.6.2 烏龜跟隨實(shí)現(xiàn)（C++）
1.編寫生成新烏龜實(shí)現(xiàn)
功能包 cpp05_exercise 的 src 目錄下，新建 C++ 文件 exer01_tf_spawn.cpp，并編輯文件，輸入如下內(nèi)容：
2.編寫坐標(biāo)變換廣播實(shí)現(xiàn)
功能包 cpp05_exercise 的 src 目錄下，新建 C++ 文件 exer02_tf_broadcaster.cpp，并編輯文件，輸入如下內(nèi)容：
3.編寫坐標(biāo)變換監(jiān)聽實(shí)現(xiàn)
功能包 cpp05_exercise 的 src 目錄下，新建 C++ 文件 exer03_tf_listener.cpp，并編輯文件，輸入如下內(nèi)容：
4.編寫 launch 文件
launch 目錄下新建文件exer01_turtle_follow.launch.py，并編輯文件，輸入如下內(nèi)容：
7.執(zhí)行
當(dāng)前工作空間下啟動(dòng)終端，輸入如下命令運(yùn)行l(wèi)aunch文件：
. install/setup.bash?
ros2 launch cpp05_exercise exer01_turtle_follow.launch.py
再新建終端，啟動(dòng) turtlesim 鍵盤控制節(jié)點(diǎn)：
ros2 run turtlesim turtle_teleop_key
該終端下可以通過鍵盤控制 turtle1 運(yùn)動(dòng)，并且 turtle2 會(huì)跟隨 turtle1 運(yùn)動(dòng)。最終的運(yùn)行結(jié)果與演示案例類似。
5.6.3 烏龜跟隨實(shí)現(xiàn)（Python）
1.編寫生成新烏龜實(shí)現(xiàn)
功能包 py05_exercise 的 py05_exercise 目錄下，新建 Python 文件 exer01_tf_spawn_py.py，并編輯文件，輸入如下內(nèi)容：
2.編寫坐標(biāo)變換廣播實(shí)現(xiàn)
功能包 py05_exercise 的 py05_exercise 目錄下，新建 Python 文件 exer02_tf_broadcaster_py.py，并編輯文件，輸入如下內(nèi)容：
功能包 py05_exercise 的 py05_exercise 目錄下，新建 Python 文件 exer03_tf_listener_py.py，并編輯文件，輸入如下內(nèi)容：
4.編寫 launch 文件
launch 目錄下新建文件 exer01_turtle_follow.launch.xml，并編輯文件，輸入如下內(nèi)容：
5.編輯配置文件
1.package.xml
在創(chuàng)建功能包時(shí)，所依賴的功能包已經(jīng)自動(dòng)配置了，配置內(nèi)容如下：
5.6.4 烏龜護(hù)航案例以及分析
1.案例需求
需求：生成多只烏龜，以 turtlesim_node 的原生烏龜（turtle1）為中心，無論 turtle1 是靜止或運(yùn)動(dòng)時(shí)，都以某種隊(duì)形為 turtle1 護(hù)航。
2.案例分析
“烏龜護(hù)航”案例的核心本質(zhì)還是坐標(biāo)變換，如果要圍繞 turtle1 組成某個(gè)固定隊(duì)形，那么可以把隊(duì)形中每個(gè)點(diǎn)位看作一個(gè)坐標(biāo)系，這些坐標(biāo)系相對于 turtle1 坐標(biāo)系的關(guān)系是靜態(tài)的，可以通過靜態(tài)廣播器發(fā)布這些點(diǎn)與 turtle1 的坐標(biāo)系關(guān)系，發(fā)布后在整個(gè)坐標(biāo)系關(guān)系樹中，點(diǎn)坐標(biāo)系到 turtle1坐標(biāo)系、turtle1坐標(biāo)系到 world 坐標(biāo)系、護(hù)航的烏龜坐標(biāo)系到 world 坐標(biāo)系的關(guān)系都是已知的，那么就可以換算出護(hù)航的烏龜坐標(biāo)系與其對應(yīng)的點(diǎn)坐標(biāo)系的相對關(guān)系，進(jìn)而就可以計(jì)算并發(fā)布控制護(hù)航烏龜運(yùn)動(dòng)的速度指令了。
3.流程簡介
本案例中生成新的烏龜、坐標(biāo)變換廣播與監(jiān)聽都已實(shí)現(xiàn)，直接編寫 launch 文件組織節(jié)點(diǎn)即可，主要步驟如下：
編寫 launch 文件集成相關(guān)節(jié)點(diǎn)；
編譯；
執(zhí)行。
案例我們會(huì)采用 C++ 和 Python 分別實(shí)現(xiàn)，二者都遵循上述實(shí)現(xiàn)流程。
5.6.5 烏龜護(hù)航實(shí)現(xiàn)（C++）
1.編寫 launch 文件
功能包 cpp05_exercise 的 launch 目錄下新建文件exer02_turtle_escort.launch.py，并編輯文件，輸入如下內(nèi)容：
5.6.6 烏龜護(hù)航實(shí)現(xiàn)（Python）
1.編寫 launch 文件
功能包 py05_exercise 的 launch 目錄下新建文件exer02_turte_escort.launch.xml，并編輯文件，輸入如下內(nèi)容：
5.7 本章小結(jié)
本章主要介紹了ROS2工具中的一個(gè)重要模塊：坐標(biāo)變換。核心內(nèi)容如下：
坐標(biāo)變換相關(guān)消息：
geometry_msgs/msg/TransformStamped和geometry_msgs/msg/PointStamped；坐標(biāo)變換廣播：靜態(tài)廣播、動(dòng)態(tài)廣播和坐標(biāo)點(diǎn)發(fā)布；
坐標(biāo)變換監(jiān)聽：坐標(biāo)系變換、坐標(biāo)點(diǎn)變換。
坐標(biāo)變換本質(zhì)還是消息的發(fā)布與訂閱，所以其框架與話題通信類似，也是由接口消息、發(fā)布方（廣播）、訂閱方（監(jiān)聽）三個(gè)要素組成的。與一般的話題通信不同的是，一般話題通信的消息發(fā)布方之間是孤立的、沒有內(nèi)在聯(lián)系，而坐標(biāo)變換的多個(gè)廣播對象廣播的消息需要被保存進(jìn)“緩存”，這些消息可以通過坐標(biāo)系id和時(shí)間戳關(guān)聯(lián)到一起，然后監(jiān)聽對象則可以通過這些內(nèi)在關(guān)聯(lián)處理緩存中的數(shù)據(jù)。除此之外，本章還介紹了坐標(biāo)變換中一些比較實(shí)用的工具，并且還通過“烏龜跟隨”與“烏龜護(hù)航”案例強(qiáng)化了坐標(biāo)變換相關(guān)API的使用。
B站有完整的ros系列教程視頻，可以觀看完整內(nèi)容ros課程ROS2理論與實(shí)踐
更多內(nèi)容將在猛獅知識星球社區(qū)更新最新課程，后續(xù)將推出更多優(yōu)質(zhì)內(nèi)容——詳情可關(guān)注猛獅集訓(xùn)營公眾號和猛獅集訓(xùn)營官方網(wǎng)站。
