# 8.3 tf in c++ ### 8.3.1 簡介 前面內容我們介紹了TF的基本的概念和TF樹消息的格式類型，我們知道，TF不僅僅是一個標準、話題，它還是一個接口。本節課我們就介紹c++中TF的一些函數和寫法。 ### 8.3.2 數據類型 C++中給我們提供了很多TF的數據類型，如下表： | 名稱 | 數據類型 | | :------: | :------: | | 向量 | tf::Vector3 | | 點 | tf::Point | | 四元數 | tf::Quaternion | | 3*3矩陣（旋轉矩陣） | tf::Matrix3x3| | 位姿 | tf::pose | | 變換 | tf::Transform | | 帶時間戳的以上類型 | tf::Stamped<T> | | 帶時間戳的變換 | tf::StampedTransform| **易混注意：**雖然此表的最后帶時間戳的變換數據類型為tf::StampedTransform,和上節我們所講的geometry_msgs/TransformStamped.msg看起來很相似，但是其實數據類型完全不一樣，tf::StampedTransform只能用在C++里，只是C++的一個類，一種數據格式，并不是一個消息。而geometry_msgs/TransformStamped.msg是一個message,它依賴于ROS，與語言無關，也即是無論何種語言，C++、Python、Java等等，都可以發送該消息。 ### 8.3.3 數據轉換  在TF里有可能會遇到各種各樣數據的轉換，例如常見的四元數、旋轉矩陣、歐拉角這三種數據之間的轉換。tf in roscpp給了我們解決該問題的函數。詳細源碼在我們教學課程的代碼包中。 首先在tf中與數據轉化的數據都類型都包含在`#include<tf/tf.h>`頭文件中，我們將與數據轉換相關API都存在tf_demo中的coordinate_transformation.cpp當中，其中列表如下： ######第1部分定義空間點和空間向量 | 編號 | 函數名稱 | 函數功能 | | :---: | :---: | :---: | | 1.1 | tfScalar::tfDot\(const Vector3 &v1, const Vector3 &v2\) | 計算兩個向量的點積 | | 1.2 | tfScalar length\(\) | 計算向量的模 | | 1.3 | Vector3 &normalize\(\) | 求與已知向量同方向的單位向量 | | 1.4 | tfScalar::tfAngle\(const Vector3 &v1, const Vector3 &v2\) | 計算兩個向量的夾角 | | 1.5 | tfScale::tfDistance\(const Vector3 &v1, const Vector3 &v2\) | 計算兩個向量的距離 | | 1.6 | tfScale::tfCross\(const Vector3 &v1,const Vector3 &v2\) | 計算兩個向量的乘積 | 示例代碼： #include <ros/ros.h> #include <tf/tf.h> //退出用：ctrl+z int main(int argc, char** argv){ //初始化 ros::init(argc, argv, "coordinate_transformation"); ros::NodeHandle node; tf::Vector3 v1(1,1,1); tf::Vector3 v2(1,0,1); //第1部分，定義空間點和空間向量 std::cout<<"第1部分，定義空間點和空間向量"<<std::endl; //1.1 計算兩個向量的點積 std::cout<<"向量v1:"<<"("<<v1[0]<<","<<v1[1]<<","<<v1[2]<<"),"; std::cout<<"向量v2:"<<"("<<v2[0]<<","<<v2[1]<<","<<v2[2]<<")"<<std::endl; std::cout<<"兩個向量的點積："<<tfDot(v1,v2)<<std::endl; //1.2 計算向量的模 std::cout<<"向量v2的模值:"<<v2.length()<<std::endl; //1.3 求與已知向量同方向的單位向量 tf::Vector3 v3; v3=v2.normalize(); std::cout<<"與向量v2的同方向的單位向量v3:"<<"("<<v3[0]<<","<<v3[1]<<","<<v3[2]<<")"<<std::endl; //1.4 計算兩個向量的夾角 std::cout<<"兩個向量的夾角(弧度):"<<tfAngle(v1,v2)<<std::endl; //1.5 計算兩個向量的距離 std::cout<<"兩個向量的距離:"<<tfDistance2(v1,v2)<<std::endl; //1.6 計算兩個向量的乘積 tf::Vector3 v4; v4=tfCross(v1,v2); std::cout<<"兩個向量的乘積v4:"<<"("<<v4[0]<<","<<v4[1]<<","<<v4[2]<<")"<<std::endl;``` return 0; } ######第2部分定義四元數 | 編號 | 函數名稱 | 函數功能 | | :---: | :---: | :---: | | 2.1 | setRPY\(const tfScalar& yaw, const stScalar &pitch, const tfScalar &roll\) | 由歐拉角計算四元數 | | 2.2 | Vector3 getAxis\(\) | 由四元數得到旋轉軸 | | 2.3 | setRotation\(const Vector3 &axis, const tfScalar& angle\) | 已知旋轉軸和旋轉角估計四元數 | 示例代碼： #include <ros/ros.h> #include <tf/tf.h> //退出用：ctrl+z int main(int argc, char** argv){ //初始化 ros::init(argc, argv, "coordinate_transformation"); ros::NodeHandle node; std::cout<<"第2部分，定義四元數"<<std::endl; //2.1 由歐拉角計算四元數 tfScalar yaw,pitch,roll; yaw=0;pitch=0;roll=0; std::cout<<"歐拉角rpy("<<roll<<","<<pitch<<","<<yaw<<")"; tf::Quaternion q; q.setRPY(yaw,pitch,roll); std::cout<<"，轉化到四元數q:"<<"("<<q[3]<<","<<q[0]<<","<<q[1]<<","<<q[2]<<")"<<std::endl; //2.2 由四元數得到旋轉軸 tf::Vector3 v5; v5=q.getAxis(); std::cout<<"四元數q的旋轉軸v5"<<"("<<v5[0]<<","<<v5[1]<<","<<v5[2]<<")"<<std::endl; //2.3 由旋轉軸和旋轉角來估計四元數 tf::Quaternion q2; q2.setRotation(v5,1.570796); std::cout<<"旋轉軸v5和旋轉角度90度，轉化到四元數q2:"<<"("<<q2[3]<<","<<q2[0]<<","<<q2[1]<<","<<q2[2]<<")"<<std::endl; return 0; } ######第3部分定義旋轉矩陣 | 編號 | 函數名稱 | 函數功能 | | :---: | :---: | :---: | | 3.1 | setRotaion\(const Quaternion &q\) | 通過四元數得到旋轉矩陣 | | 3.2 | getEulerYPR\(tfScalar &yaw, tfScalar &pitch, tfScalar &roll \) | 由旋轉矩陣求歐拉角 | 示例代碼： #include <ros/ros.h> #include <tf/tf.h> //退出用：ctrl+z int main(int argc, char** argv){ //初始化 ros::init(argc, argv, "coordinate_transformation"); ros::NodeHandle node; //第3部分，定義旋轉矩陣 std::cout<<"第3部分，定義旋轉矩陣"<<std::endl; //3.1 由旋轉軸和旋轉角來估計四元數 tf::Quaternion q2(1,0，0,0）; tf::Matrix3x3 Matrix; tf::Vector3 v6,v7,v8; Matrix.setRotation(q2); v6=Matrix[0]; v7=Matrix[1]; v8=Matrix[2]; std::cout<<"四元數q2對應的旋轉矩陣M:"<<v6[0]<<","<<v6[1]<<","<<v6[2]<<std::endl; std::cout<<" "<<v7[0]<<","<<v7[1]<<","<<v7[2]<<std::endl; std::cout<<" "<<v8[0]<<","<<v8[1]<<","<<v8[2]<<std::endl; //3.2 通過旋轉矩陣求歐拉角 tfScalar m_yaw,m_pitch,m_roll; Matrix.getEulerYPR(m_yaw,m_pitch,m_roll); std::cout<<"由旋轉矩陣M,得到歐拉角rpy("<<m_roll<<","<<m_pitch<<","<<m_yaw<<")"<<std::endl; return 0; }; 此外，在tf_demo的教學包中，我們還提供常見的歐拉角與四元數的互換，詳見Euler2Quaternion.cpp與Quaternion2Euler.cpp Euler2Quaternion.cpp #include <ros/ros.h> #include <tf/tf.h> //退出用：ctrl+z int main(int argc, char** argv){ //初始化 ros::init(argc, argv, "Euler2Quaternion"); ros::NodeHandle node; geometry_msgs::Quaternion q; double roll,pitch,yaw; while(ros::ok()) { //輸入一個相對原點的位置 std::cout<<"輸入的歐拉角：roll,pitch,yaw:"; std::cin>>roll>>pitch>>yaw; //輸入歐拉角，轉化成四元數在終端輸出 q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw); //ROS_INFO("輸出的四元數為：w=%d,x=%d,y=%d,z=%d"，q.w,q.x,q.y,q.z); std::cout<<"輸出的四元數為：w="<<q.w<<",x="<<q.x<<",y="<<q.y<<",z="<<q.z<<std::endl; ros::spinOnce(); } return 0; }; Quaternion2Euler.cpp #include <ros/ros.h> #include "nav_msgs/Odometry.h" #include <tf/tf.h> //退出用：ctrl+z int main(int argc, char** argv){ //初始化 ros::init(argc, argv, "Quaternion2Euler"); ros::NodeHandle node; nav_msgs::Odometry position; tf::Quaternion RQ2; double roll,pitch,yaw; while(ros::ok()) { //輸入一個相對原點的位置 std::cout<<"輸入的四元數：w,x,y,z:"; std::cin>>position.pose.pose.orientation.w>>position.pose.pose.orientation.x>>position.pose.pose.orientation.y>>position.pose.pose.orientation.z; //輸入四元數，轉化成歐拉角數在終端輸出 tf::quaternionMsgToTF(position.pose.pose.orientation,RQ2); // tf::Vector3 m_vector3; 方法2 // m_vector3=RQ2.getAxis(); tf::Matrix3x3(RQ2).getRPY(roll,pitch,yaw); std::cout<<"輸出的歐拉角為：roll="<<roll<<",pitch="<<pitch<<",yaw="<<yaw<<std::endl; //std::cout<<"輸出歐拉角為：roll="<<m_vector3[0]<<",pitch="<<m_vector3[1]<<",yaw="<<m_vector3[2]<<std::endl; ros::spinOnce(); } return 0; }; ### 8.3.4 TF類 ##### tf::TransformBroadcaster類 transformBroadcaster() void sendTransform(const StampedTransform &transform) void sendTransform(const std::vector<StampedTransform> &transforms) void sendTransform(const geometry_msgs::TransformStamped &transform) void sendTransform(const std::vector<geometry_msgs::TransformStamped> &transforms) 這個類在前面講TF樹的時候提到過，這個broadcaster就是一個publisher,而sendTransform的作用是來封裝publish的函數。在實際的使用中，我們需要在某個Node中構建tf::TransformBroadcaster類，然后調用sendTransform(),將transform發布到`/tf`的一段transform上。`/tf`里的transform為我們重載了多種不同的函數類型。在我們的tf_demo教學包當中提供了相關的示例代碼tf.broadcaster.cpp，具體如下： #include <ros/ros.h> #include <tf/transform_broadcaster.h> #include <tf/tf.h> //退出用：ctrl+z int main(int argc, char** argv){ //初始化 ros::init(argc, argv, "tf_broadcaster"); ros::NodeHandle node; static tf::TransformBroadcaster br; tf::Transform transform; //geometry_msgs::Quaternion qw; tf::Quaternion q; //定義初始坐標和角度 double roll=0,pitch=0,yaw=0,x=1.0,y=2.0,z=3.0; ros::Rate rate(1); while(ros::ok()) { yaw+=0.1;//每經過一秒開始一次變換 //輸入歐拉角，轉化成四元數在終端輸出 q.setRPY(roll,pitch,yaw); //qw=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);方法2 transform.setOrigin(tf::Vector3(x,y,z)); transform.setRotation(q); std::cout<<"發布tf變換：sendTransform函數"<<std::endl; br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"base_link","link1")); std::cout<<"輸出的四元數為：w="<<q[3]<<",x="<<q[0]<<",y="<<q[1]<<",z="<<q[2]<<std::endl; // std::cout<<"輸出的四元數為：w="<<qw.w<<",x="<<qw.x<<",y="<<qw.y<<",z="<<qw.z<<std::endl; rate.sleep(); ros::spinOnce(); } return 0; }; ##### tf::TransformListener類 void lookupTranform(const std::string &target_frame,const std::string &source_frame,const ros::Time &time,StampedTransform &transform)const bool canTransform() bool waitForTransform()const 上一個類是向`/tf`上發的類，那么這一個就是從`/tf`上接收的類。首先看lookuptransform()函數，第一個參數是目標坐標系，第二個參數為源坐標系，也即是得到從源坐標系到目標坐標系之間的轉換關系，第三個參數為查詢時刻，第四個參數為存儲轉換關系的位置。值得注意，第三個參數通常用`ros::Time（0）`,這個表示為最新的坐標轉換關系，而`ros::time::now`則會因為收發延遲的原因，而不能正確獲取當前最新的坐標轉換關系。canTransform()是用來判斷兩個transform之間是否連通，waitForTransform()const是用來等待某兩個transform之間的連通，在我們的tf_demo教學包當中提供了相關的示例代碼tf_listerner.cpp，具體如下： #include <ros/ros.h> #include <tf/transform_listener.h> #include <geometry_msgs/Twist.h> int main(int argc, char** argv){ ros::init(argc, argv, "tf_listener"); ros::NodeHandle node; tf::TransformListener listener; //1. 阻塞直到frame相通 std::cout<<"1. 阻塞直到frame相通"<<std::endl; listener.waitForTransform("/base_link","link1",ros::Time(0),ros::Duration(4.0)); ros::Rate rate(1); while (node.ok()){ tf::StampedTransform transform; try{ //2. 監聽對應的tf,返回平移和旋轉 std::cout<<"2. 監聽對應的tf,返回平移和旋轉"<<std::endl; listener.lookupTransform("/base_link", "/link1", ros::Time(0), transform); //ros::Time(0)表示最近的一幀坐標變換，不能寫成ros::Time::now() } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); continue; } std::cout<<"輸出的位置坐標：x="<<transform.getOrigin().x()<<",y="<<transform.getOrigin().y()<<",z="<<transform.getOrigin().z()<<std::endl; std::cout<<"輸出的旋轉四元數：w="<<transform.getRotation().getW()<<",x="<<transform.getRotation().getX()<< ",y="<<transform.getRotation().getY()<<",z="<<transform.getRotation().getZ()<<std::endl; rate.sleep(); } return 0; };