tf樹 tf變換（1）
tf變換（1）
TF庫的目的是實現系統中任一個點在所有座標系之間的座標變換，也就是說，只要給定一個座標系下的一個點的座標，就能獲得這個點在其他座標系的座標.
使用tf功能包，a. 監聽tf變換： 接收並快取系統中釋出的所有參考系變換，並從中查詢所需要的參考系變換。
b.廣播 tf變換: 向系統中廣播參考系之間的座標變換關係。系統中更可能會存在多個不同部分的tf變換廣播，每個廣播都可以直接將參考系變換關係直接插入tf樹中，不需要再進行同步。
首先介紹關於TF的API的一些資料結構：
基本的資料型別有（Quaternion, Vector, Point, Pose, Transform）
這其中Quaternion 是表示四元數，vector3是一個3*1 的向量，point是一個表示一個點座標，Pose是位姿（位姿是包括座標以及方向） Transform是一個轉換的模版
tf::Stamped <T>
是一種包含了除了Transform的其他幾種基本的資料結構的一種資料結構:
template <typename T> //模版結構可以是tf::Pose tf:Point 這些基本的結構 class Stamped : public T{ public: ros::Time stamp_; //記錄時間 std::string frame_id_; //ID Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation Stamped(const T& input, const ros::Time& timestamp, conststd::string & frame_id); void setData(const T& input); };
tf::StampedTransform
TF::stampedtransform是TF的一種特殊情況：它需要frame_id和stamp以及child_frame_id。
/** \brief The Stamped Transform datatype used by tf */ class StampedTransform : public tf::Transform { public: ros::Time stamp_; ///< The timestamp associated with this transform 時間戳 std::string frame_id_; ///< The frame_id of the coordinate frame in which this transform is defined 定義轉換座標框架的frameID std::string child_frame_id_; ///< The frame_id of the coordinate frame this transform defines 的座標系變換定義的id StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id): tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ }; /** \brief Default constructor only to be used for preallocation */ StampedTransform() { }; /** \brief Set the inherited Traonsform data */ void setData(const tf::Transform& input){*static_cast<tf::Transform*>(this) = input;}; };
舉個例子
在機器人的定位領域有蒙特卡羅定位（AMCL）的演算法，這個演算法是根據給定的地圖，結合粒子濾波獲取最佳定位點Mp，這個定位點是相對於地圖map上的座標,也就是base_link（也就是機器人的基座標）相對map上的座標。我們知道 odom 的原點是機器人啟動時刻的位置，它在map上的位置或轉換矩陣是未知的。但是AMCL可以根據最佳粒子的位置推算出 odom->map（就是說通過最佳粒子推算出機器人在地圖的中的位置）的tf轉換資訊併發布到 tf主題上。因為base_link->odom的tf轉換資訊是每時每刻都在釋出的，所以它是已知的
，所以這裡有個這樣的tf關係
map->base_link（就是地圖中機器人的位置 是根據最佳粒子推算的）
base_link->odom（這是現實中機器人的位姿可以說是里程計的資訊）
可以理解：機器人的里程計的資訊 = 當前地圖中的機器人的的位置 減去 地圖中機器人的起點位置。
轉為公式可以寫成 ：map->odom = map->base_link - base_link->odom
或者寫為：
base_link->odom = map->base_link - map->odom （這樣更容易理解）
提示：首先我們可以先了解關於PRY這三個概念關於pitch yaw roll的部落格 http://blog.csdn.net/yuzhongchun/article/details/22749521）
pitch是圍繞X軸旋轉，也叫做俯仰角，
yaw是圍繞Y軸旋轉，也叫偏航角，
roll是圍繞Z軸旋轉，也叫翻滾角
1. ROS_DEBUG("New pose: %6.3f %6.3f %6.3f", 2. hyps[max_weight_hyp].pf_pose_mean.v[0], 3. hyps[max_weight_hyp].pf_pose_mean.v[1], 4. hyps[max_weight_hyp].pf_pose_mean.v[2]); 5. // hyps[max_weight_hyp].pf_pose_mean.v[0], [1], [2] 就代表了Mp 也就是機器人的位姿那麼位姿的格式是（x,y,theta）最後一個是yaw偏航角， 6. // subtracting base to odom from map to base and send map to odom instead 7. tf::Stamped<tf::Pose> odom_to_map; 8. try 9. { 10. tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]), tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0], 11. hyps[max_weight_hyp].pf_pose_mean.v[1], 12. 0.0)); 13. // tmp_tf = 從map原點看base_link的位置 為yaw生成四元數，最後的0.0是（x,y,z）的Z的值為0 因為這是在二維平面中。 14. tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(), 15. laser_scan->header.stamp, 16. base_frame_id_); 17. //tmp_tf.inverse() = 以為Mp為座標的原點，地圖map原點相對於Mp的位置，就是在base_link座標系下map 原點的位置 18. this->tf_->transformPose(odom_frame_id_, 19. tmp_tf_stamped, 20. odom_to_map); 21. //進行 base_link座標系下的點轉換到odom座標系，也就是把map原點轉換到odom座標系下，等於從odom原點看map原點的位置。放到latest_tf_變數裡面 22. } 23. catch(tf::TransformException) 24. { 25. ROS_DEBUG("Failed to subtract base to odom transform"); 26. return; 27. }
TF命令列工具
（1） tf_monitor工具的功能是列印tf樹中的所有參考系資訊，通過輸入引數來檢視指定參考系之間的資訊 用法： rosrun tf tf_monitor
tf_monitor <source_frame> <target_target> 監視一個特定的轉換 For example, to monitor the transform from /base_footprint to /odom:
（2） tf_echo工具的功能是檢視指定參考系之間的變換關係。命令的格式： tf_echo <source_frame> <target_frame>
（3）static_transform_publisher工具的功能是釋出兩個參考系之間的靜態座標變換，兩個參考系一般不發生相對位置變化
（4）view_frames 是視覺化的除錯工具，可以生成pdf檔案，來顯示整棵tf樹的資訊。用法：rosrun tf view_frames
具體可以檢視http://wiki.ros.org/tf/
TF庫的目的是實現系統中任一個點在所有座標系之間的座標變換，也就是說，只要給定一個座標系下的一個點的座標，就能獲得這個點在其他座標系的座標.
使用tf功能包，a. 監聽tf變換： 接收並快取系統中釋出的所有參考系變換，並從中查詢所需要的參考系變換。
b.廣播 tf變換: 向系統中廣播參考系之間的座標變換關係。系統中更可能會存在多個不同部分的tf變換廣播，每個廣播都可以直接將參考系變換關係直接插入tf樹中，不需要再進行同步。
首先介紹關於TF的API的一些資料結構：
基本的資料型別有（Quaternion, Vector, Point, Pose, Transform）
這其中Quaternion 是表示四元數，vector3是一個3*1 的向量，point是一個表示一個點座標，Pose是位姿（位姿是包括座標以及方向） Transform是一個轉換的模版
tf::Stamped <T>
是一種包含了除了Transform的其他幾種基本的資料結構的一種資料結構:
template <typename T> //模版結構可以是tf::Pose tf:Point 這些基本的結構 class Stamped : public T{ public: ros::Time stamp_; //記錄時間 std::string frame_id_; //ID Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id); void setData(const T& input); };
tf::StampedTransform
TF::stampedtransform是TF的一種特殊情況：它需要frame_id和stamp以及child_frame_id。
/** \brief The Stamped Transform datatype used by tf */ class StampedTransform : public tf::Transform { public: ros::Time stamp_; ///< The timestamp associated with this transform 時間戳 std::string frame_id_; ///< The frame_id of the coordinate frame in which this transform is defined 定義轉換座標框架的frameID std::string child_frame_id_; ///< The frame_id of the coordinate frame this transform defines 的座標系變換定義的id StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id): tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ }; /** \brief Default constructor only to be used for preallocation */ StampedTransform() { }; /** \brief Set the inherited Traonsform data */ void setData(const tf::Transform& input){*static_cast<tf::Transform*>(this) = input;}; };
舉個例子
在機器人的定位領域有蒙特卡羅定位（AMCL）的演算法，這個演算法是根據給定的地圖，結合粒子濾波獲取最佳定位點Mp，這個定位點是相對於地圖map上的座標,也就是base_link（也就是機器人的基座標）相對map上的座標。我們知道 odom 的原點是機器人啟動時刻的位置，它在map上的位置或轉換矩陣是未知的。但是AMCL可以根據最佳粒子的位置推算出 odom->map（就是說通過最佳粒子推算出機器人在地圖的中的位置）的tf轉換資訊併發布到 tf主題上。因為base_link->odom的tf轉換資訊是每時每刻都在釋出的，所以它是已知的
，所以這裡有個這樣的tf關係
map->base_link（就是地圖中機器人的位置 是根據最佳粒子推算的）
base_link->odom（這是現實中機器人的位姿可以說是里程計的資訊）
可以理解：機器人的里程計的資訊 = 當前地圖中的機器人的的位置 減去 地圖中機器人的起點位置。
轉為公式可以寫成 ：map->odom = map->base_link - base_link->odom
或者寫為：
base_link->odom = map->base_link - map->odom （這樣更容易理解）
提示：首先我們可以先了解關於PRY這三個概念關於pitch yaw roll的部落格 http://blog.csdn.net/yuzhongchun/article/details/22749521）
pitch是圍繞X軸旋轉，也叫做俯仰角，
yaw是圍繞Y軸旋轉，也叫偏航角，
roll是圍繞Z軸旋轉，也叫翻滾角
1. ROS_DEBUG("New pose: %6.3f %6.3f %6.3f", 2. hyps[max_weight_hyp].pf_pose_mean.v[0], 3. hyps[max_weight_hyp].pf_pose_mean.v[1], 4. hyps[max_weight_hyp].pf_pose_mean.v[2]); 5. // hyps[max_weight_hyp].pf_pose_mean.v[0], [1], [2] 就代表了Mp 也就是機器人的位姿那麼位姿的格式是（x,y,theta）最後一個是yaw偏航角， 6. // subtracting base to odom from map to base and send map to odom instead 7. tf::Stamped<tf::Pose> odom_to_map; 8. try 9. { 10. tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]), tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0], 11. hyps[max_weight_hyp].pf_pose_mean.v[1], 12. 0.0)); 13. // tmp_tf = 從map原點看base_link的位置 為yaw生成四元數，最後的0.0是（x,y,z）的Z的值為0 因為這是在二維平面中。 14. tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(), 15. laser_scan->header.stamp, 16. base_frame_id_); 17. //tmp_tf.inverse() = 以為Mp為座標的原點，地圖map原點相對於Mp的位置，就是在base_link座標系下map 原點的位置 18. this->tf_->transformPose(odom_frame_id_, 19. tmp_tf_stamped, 20. odom_to_map); 21. //進行 base_link座標系下的點轉換到odom座標系，也就是把map原點轉換到odom座標系下，等於從odom原點看map原點的位置。放到latest_tf_變數裡面 22. } 23. catch(tf::TransformException) 24. { 25. ROS_DEBUG("Failed to subtract base to odom transform"); 26. return; 27. }
TF命令列工具
（1） tf_monitor工具的功能是列印tf樹中的所有參考系資訊，通過輸入引數來檢視指定參考系之間的資訊 用法： rosrun tf tf_monitor
tf_monitor <source_frame> <target_target> 監視一個特定的轉換 For example, to monitor the transform from /base_footprint to /odom:
（2） tf_echo工具的功能是檢視指定參考系之間的變換關係。命令的格式： tf_echo <source_frame> <target_frame>
（3）static_transform_publisher工具的功能是釋出兩個參考系之間的靜態座標變換，兩個參考系一般不發生相對位置變化
（4）view_frames 是視覺化的除錯工具，可以生成pdf檔案，來顯示整棵tf樹的資訊。用法：rosrun tf view_frames
具體可以檢視http://wiki.ros.org/tf/