《鴻蒙機(jī)器人編程》8-ROS坐標(biāo)變換系統(tǒng)TF-教案

ROS坐標(biāo)轉(zhuǎn)換系統(tǒng)(tf)ROStf概念展示機(jī)器人系統(tǒng)一般都包含很多坐標(biāo)系，如世界坐標(biāo)系，機(jī)體坐標(biāo)系，機(jī)械手坐標(biāo)系，頭坐標(biāo)系等。tf是一個(gè)讓使用者可以跟蹤所有這些坐標(biāo)系，它采用樹(shù)型數(shù)據(jù)結(jié)構(gòu)，根據(jù)時(shí)間緩沖并維護(hù)各個(gè)參考系的坐標(biāo)變換關(guān)系，可以在任意時(shí)間完成2個(gè)參考系的坐標(biāo)變換。常用操作有監(jiān)聽(tīng)坐標(biāo)變換和廣播坐標(biāo)變換。通過(guò)一個(gè)例子展示tf的概念。首先在終端輸入下面的命令，安裝相應(yīng)的包：$sudoaptinstallros-noetic-ros-tutorialsros-noetic-geometry-tutorialsros-noetic-rvizros-noetic-rosbashros-noetic-rqt-tf-tree然后運(yùn)行下面的命令：$sudoupdate-alternatives--install/usr/bin/pythonpython/usr/bin/python3.81移動(dòng)鍵盤(pán)的箭頭鍵，會(huì)發(fā)現(xiàn)后一個(gè)烏龜跟著前一個(gè)烏龜運(yùn)動(dòng)：$roslaunchturtle_tfturtle_tf_demo.launch這個(gè)例子應(yīng)用tf庫(kù)建立了三個(gè)坐標(biāo)系，world坐標(biāo)系，turtle1坐標(biāo)系，turtle2坐標(biāo)系。tf提供了幾個(gè)常用工具：（1）view_frames創(chuàng)建一個(gè)在ROS上被tf廣播的圖表，運(yùn)行命令如下：$rosruntfview_frames它會(huì)監(jiān)聽(tīng)在ROS上廣播的坐標(biāo)系，然后畫(huà)一顆這些坐標(biāo)系怎么連接的樹(shù)形圖。打開(kāi)生成的pdf文件：$evinceframes.pdf（2）rqt_tf_tree用于查看在ROS上廣播的坐標(biāo)軸的樹(shù)形圖的圖形化工具，可以手動(dòng)刷新。$rosrunrqt_tf_treerqt_tf_tree（3）tf_echo命令用于顯示兩個(gè)坐標(biāo)軸的轉(zhuǎn)換關(guān)系。$rosruntftf_echo[reference_frame][target_frame]$rosruntftf_echoturtle1turtle2此外，在rviz里面也可以顯示tf的信息。運(yùn)行下列命令：$rosrunrvizrviz-d`rospackfindturtle_tf`/rviz/turtle_rviz.rviz$roslaunchturtle_tfturtle_tf_demo.launch這是運(yùn)行界面截圖：ROStf的--廣播的程序首先創(chuàng)建ROS功能包learning_tf。在src目錄下建立如下cpp文件turtle_tf_broadcaster.cpp。$cd%YOUR_CATKIN_WORKSPACE_HOME%/src$catkin_create_pkglearning_tftfroscpprospyturtlesim$cdlearning_tf詳細(xì)代碼如下：程序訂閱了turtle_name/pose話題，turtle_name是程序運(yùn)行的輸入?yún)?shù)。statictf::TransformBroadcasterbr;tf是一個(gè)命名空間，詳細(xì)的文檔介紹在如下網(wǎng)址：/kinetic/api/tf/html/c++/namespacetf.html。TransformBroadcaster是tf命名空間下的一個(gè)類(lèi)，這個(gè)類(lèi)提供一個(gè)簡(jiǎn)單的方法去發(fā)布坐標(biāo)轉(zhuǎn)換信息，處理說(shuō)有的發(fā)布和填充消息。常用成員函數(shù)：voidsendTransform(constStampedTransform&transform)StampedTransform類(lèi)：tf的采樣轉(zhuǎn)換的數(shù)據(jù)類(lèi)型。常用成員函數(shù)：StampedTransform(consttf::Transform&input,constros::Time×tamp,conststd::string&frame_id,conststd::string&child_frame_id)Quaternion類(lèi)，常規(guī)的四元數(shù)類(lèi)，表示剛體的旋轉(zhuǎn)。Transform類(lèi)，支持剛體的平移和旋轉(zhuǎn)變換。tf::Transformtransform;transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));tf::Quaternionq;q.setRPY(0,0,msg->theta);transform.setRotation(q);建立了一個(gè)tf::Transform實(shí)例，然后把turtle2D的pose信息轉(zhuǎn)換成3D的信息。br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));第一個(gè)參數(shù)是transform實(shí)例，第二個(gè)是時(shí)間戳，第三個(gè)是父坐標(biāo)系名，第四個(gè)是子坐標(biāo)系名。打開(kāi)該package的CMakeLists.txt文件，添加下面命令代碼：add_executable(turtle_tf_broadcastersrc/turtle_tf_broadcaster.cpp)target_link_libraries(turtle_tf_broadcaster${catkin_LIBRARIES})然后編譯工作區(qū)：$catkin_make編譯成功后，編寫(xiě)如下的launch文件：<launch><!--TurtlesimNode--><nodepkg="turtlesim"type="turtlesim_node"name="sim"/><nodepkg="turtlesim"type="turtle_teleop_key"name="teleop"output="screen"/><!--Axes--><paramname="scale_linear"value="2"type="double"/><paramname="scale_angular"value="2"type="double"/><nodepkg="learning_tf"type="turtle_tf_broadcaster"args="/turtle1"name="turtle1_tf_broadcaster"/><nodepkg="learning_tf"type="turtle_tf_broadcaster"args="/turtle2"name="turtle2_tf_broadcaster"/></launch>然后運(yùn)行該launch文件：$roslaunchlearning_tfstart_demo.launch檢驗(yàn)下結(jié)果：$rosruntftf_echo/world/turtle1下面是廣播tf的程序運(yùn)行結(jié)果：ROStf--tf訂閱者首先切換到learning_tf目錄下：$roscdlearning_tf在src目錄下建立src/turtle_tf_listener.cpp文件。tf::Transformer類(lèi)，提供一個(gè)系統(tǒng)內(nèi)的任意兩個(gè)坐標(biāo)系的轉(zhuǎn)換。tf::TransformListener類(lèi)，繼承tf::Transformer類(lèi)，自動(dòng)訂閱ROS的transform消息。常用成員函數(shù)：voidTransformer::lookupTransform(conststd::string&target_frame,conststd::string&source_frame,constros::Time&time,StampedTransform&transform)const通過(guò)frameID得到兩個(gè)坐標(biāo)系的轉(zhuǎn)換關(guān)系。target_frame目標(biāo)坐標(biāo)系：需要將數(shù)據(jù)變換到的坐標(biāo)系。source_frame起始坐標(biāo)系：數(shù)據(jù)的起始坐標(biāo)系。time時(shí)間：變換所需的時(shí)間（0表示時(shí)間待定）。transform變換：添加變換參考。tf::TransformListenerlistener;try{listener.lookupTransform("/turtle2","/turtle1",ros::Time(0),transform);}建立一個(gè)TransformListener

實(shí)例，一旦實(shí)例創(chuàng)建，它就自動(dòng)接收tf的轉(zhuǎn)換消息，并緩存10秒。然后查詢一個(gè)指定的轉(zhuǎn)換，就是查詢從/turtle2到/turtle1的轉(zhuǎn)換，ros::Time(0)

表示我們想要的是最新的一次變換，然后把變換存到transform變量。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);根據(jù)上面檢查的變換矩陣，把位置變成線速度，把角度變成角速度，然后發(fā)布turtle2/cmd_vel話題，驅(qū)動(dòng)turtle2走向turtle1。然后在CMakeLists.txt中加入如下代碼：add_executable(turtle_tf_listenersrc/turtle_tf_listener.cpp)target_link_libraries(turtle_tf_listener${catkin_LIBRARIES})然后進(jìn)行編譯：$catkin_make然后編寫(xiě)launch文件，代碼如下：<launch><!--TurtlesimNode--><nodepkg="turtlesim"type="turtlesim_node"name="sim"/><nodepkg="turtlesim"type="turtle_teleop_key"name="teleop"output="screen"/><!--Axes--><paramname="scale_linear"value="2"type="double"/><paramname="scale_angular"value="2"type="double"/><nodepkg="learning_tf"type="turtle_tf_broadcaster"args="/turtle1"name="turtle1_tf_broadcaster"/><nodepkg="learning_tf"type="turtle_tf_broadcaster"args="/turtle2"name="turtle2_tf_broadcaster"/><nodepkg="learning_tf"type="turtle_tf_listener"name="listener"/></launch>然后運(yùn)行代碼：$roslaunchlearning_tfstart_demo.launch效果如圖所示：機(jī)器人tf實(shí)戰(zhàn)很多ros包都要求用tf軟件庫(kù)發(fā)布機(jī)器人的變換樹(shù)，一個(gè)變換樹(shù)定義了在不同坐標(biāo)系間的平移和旋轉(zhuǎn)。舉例來(lái)說(shuō)，一個(gè)機(jī)器人有一個(gè)移動(dòng)底盤(pán)和一個(gè)安裝在它上面的激光雷達(dá)。這樣，我們可以定義兩個(gè)坐標(biāo)系，一個(gè)在機(jī)器人底部的中心（base_link)，一個(gè)在激光雷達(dá)的中心(base_laser）。對(duì)于導(dǎo)航包來(lái)說(shuō)，base_link就是機(jī)器人的旋轉(zhuǎn)中心?，F(xiàn)在我們有一些來(lái)自laser的距離數(shù)據(jù)，是在base_laser坐標(biāo)系下的，現(xiàn)在我們要把這個(gè)數(shù)據(jù)轉(zhuǎn)換到base_link下，幫助機(jī)器人避障。假設(shè)我們知道激光在機(jī)器人底部中心的前方10cm上方20cm，沒(méi)有旋轉(zhuǎn)，如果我們?cè)赽ase_laser下測(cè)得的距離是(0.3,0.0,0.0),那么在base_link下距離為(0.4,0.0,0.2).在catkin_ws/src目錄輸入如下命令：$catkin_create_pkgrobot_setup_tfroscpptfgeometry_msgs接下來(lái)我們將在ros下廣播一個(gè)base_laser->base_link的變換，在robot_setup_tf包下加上以下代碼：#include<ros/ros.h>#include<tf/transform_broadcaster.h>intmain(intargc,char**argv){ros::init(argc,argv,"robot_tf_publisher");ros::NodeHandlen;ros::Rater(100);tf::TransformBroadcasterbroadcaster;while(n.ok()){ broadcaster.sendTransform(tf::StampedTransform(tf::Transform(tf::Quaternion(0,0,0,1),tf::Vector3(0.1,0.0,0.2)),ros::Time::now(),"base_link","base_laser"));r.sleep();}}我們手動(dòng)生成了一個(gè)點(diǎn)的激光數(shù)據(jù)，(1.0,0.2,0.0)：//we'llcreateapointinthebase_laserframethatwe'dliketotransformtothebase_linkframegeometry_msgs::PointStampedlaser_point;laser_point.header.frame_id="base_laser";//we'lljustusethemostrecenttransformavailableforoursimpleexamplelaser_point.header.stamp=ros::Time();//justanarbitrarypointinspacelaser_point.point.x=1.0;laser_point.point.y=0.2;laser_point.point.z=0.0;把base_laser的數(shù)據(jù)轉(zhuǎn)到base_link下：try{geometry_msgs::PointStampedbase_point;listener.transformPoint("base_link",laser_point,base_point);ROS_INFO("base_laser:(%.2f,%.2f.%.2f)>base_link:(%.2f,%.2f,%.2f)attime%.2f",laser_point.p