5.1 TF座標變換
5.1 TF座標變換
機器人系統上，有多個感測器，如鐳射雷達、攝像頭等，有的感測器是可以感知機器人周邊的物體方位(或者稱之為:座標，橫向、縱向、高度的距離資訊)的，以協助機器人定位障礙物，可以直接將物體相對該感測器的方位資訊，等價於物體相對於機器人系統或機器人其它元件的方位資訊嗎？顯示是不行的，這中間需要一個轉換過程。更具體描述如下:
場景1:雷達與小車
現有一移動式機器人底盤，在底盤上安裝了一雷達，雷達相對於底盤的偏移量已知，現雷達檢測到一障礙物資訊，獲取到座標分別為(x,y,z)，該座標是以雷達為參考系的，如何將這個座標轉換成以小車為參考系的座標呢？
場景2:現有一帶機械臂的機器人(比如:PR2)需要夾取目標物，當前機器人頭部攝像頭可以探測到目標物的座標(x,y,z)，不過該座標是以攝像頭為參考系的，而實際操作目標物的是機械臂的夾具，當前我們需要將該座標轉換成相對於機械臂夾具的座標，這個過程如何實現？
當然，根據我們高中學習的知識，在明確了不同座標系之間的的相對關係，就可以實現任何座標點在不同座標系之間的轉換，但是該計算實現是較為常用的，且演算法也有點複雜，因此在 ROS 中直接封裝了相關的模組: 座標變換(TF)。
概念
tf:TransForm Frame,座標變換
座標系:ROS 中是通過座標系統開標定物體的，確切的將是通過右手座標系來標定的。
作用
在 ROS 中用於實現不同座標系之間的點或向量的轉換。
案例
小烏龜跟隨案例：如本章引言部分演示。
說明
在ROS中座標變換最初對應的是tf，不過在 hydro 版本開始, tf 被棄用，遷移到 tf2,後者更為簡潔高效，tf2對應的常用功能包有:
tf2_geometry_msgs:可以將ROS訊息轉換成tf2訊息。
tf2: 封裝了座標變換的常用訊息。
tf2_ros:為tf2提供了roscpp和rospy繫結，封裝了座標變換常用的API。
另請參考:
5.1.1 座標msg訊息
訂閱釋出模型中資料載體 msg 是一個重要實現，首先需要了解一下，在座標轉換實現中常用的 msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped
前者用於傳輸座標系相關位置資訊，後者用於傳輸某個座標系內座標點的資訊。在座標變換中，頻繁的需要使用到座標系的相對關係以及座標點資訊。
1.geometry_msgs/TransformStamped
命令列鍵入:rosmsg info geometry_msgs/TransformStamped
std_msgs/Header header #頭資訊
uint32 seq #|-- 序列號
time stamp #|-- 時間戳
string frame_id #|-- 座標 ID
string child_frame_id #子座標系的 id
geometry_msgs/Transform transform #座標資訊
geometry_msgs/Vector3 translation #偏移量
float64 x #|-- X 方向的偏移量
float64 y #|-- Y 方向的偏移量
float64 z #|-- Z 方向上的偏移量
geometry_msgs/Quaternion rotation #四元數
float64 x
float64 y
float64 z
float64 w
四元數用於表示座標的相對姿態
2.geometry_msgs/PointStamped
命令列鍵入:rosmsg info geometry_msgs/PointStamped
std_msgs/Header header #頭
uint32 seq #|-- 序號
time stamp #|-- 時間戳
string frame_id #|-- 所屬座標系的 id
geometry_msgs/Point point #點座標
float64 x #|-- x y z 座標
float64 y
float64 z
另請參考:
-
http://docs.ros.org/en/api/geometry_msgs/html/msg/TransformStamped.html
-
http://docs.ros.org/en/api/geometry_msgs/html/msg/PointStamped.html
5.1.2 靜態座標變換
所謂靜態座標變換，是指兩個座標系之間的相對位置是固定的。
需求描述:
現有一機器人模型，核心構成包含主體與雷達，各對應一座標系，座標系的原點分別位於主體與雷達的物理中心，已知雷達原點相對於主體原點位移關係如下: x 0.2 y0.0 z0.5。當前雷達檢測到一障礙物，在雷達座標系中障礙物的座標為 (2.0 3.0 5.0),請問，該障礙物相對於主體的座標是多少？
結果演示:
實現分析:
- 座標系相對關係，可以通過釋出方釋出
- 訂閱方，訂閱到釋出的座標系相對關係，再傳入座標點資訊(可以寫死)，然後藉助於 tf 實現座標變換，並將結果輸出
實現流程:C++ 與 Python 實現流程一致
- 新建功能包，新增依賴
- 編寫釋出方實現
- 編寫訂閱方實現
- 執行並檢視結果
方案A:C++實現
1.建立功能包
建立專案功能包依賴於 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
2.釋出方
/*
靜態座標變換髮布方:
釋出關於 laser 座標系的位置資訊
實現流程:
1.包含標頭檔案
2.初始化 ROS 節點
3.建立靜態座標轉換廣播器
4.建立座標系資訊
5.廣播器釋出座標系資訊
6.spin()
*/
// 1.包含標頭檔案
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 節點
ros::init(argc,argv,"static_brocast");
// 3.建立靜態座標轉換廣播器
tf2_ros::StaticTransformBroadcaster broadcaster;
// 4.建立座標系資訊
geometry_msgs::TransformStamped ts;
//----設定頭資訊
ts.header.seq = 100;
ts.header.stamp = ros::Time::now();
ts.header.frame_id = "base_link";
//----設定子級座標系
ts.child_frame_id = "laser";
//----設定子級相對於父級的偏移量
ts.transform.translation.x = 0.2;
ts.transform.translation.y = 0.0;
ts.transform.translation.z = 0.5;
//----設定四元數:將 尤拉角資料轉換成四元數
tf2::Quaternion qtn;
qtn.setRPY(0,0,0);
ts.transform.rotation.x = qtn.getX();
ts.transform.rotation.y = qtn.getY();
ts.transform.rotation.z = qtn.getZ();
ts.transform.rotation.w = qtn.getW();
// 5.廣播器釋出座標系資訊
broadcaster.sendTransform(ts);
ros::spin();
return 0;
}
配置檔案此處略。
3.訂閱方
/*
訂閱座標系資訊，生成一個相對於 子級座標系的座標點資料，轉換成父級座標系中的座標點
實現流程:
1.包含標頭檔案
2.初始化 ROS 節點
3.建立 TF 訂閱節點
4.生成一個座標點(相對於子級座標系)
5.轉換座標點(相對於父級座標系)
6.spin()
*/
//1.包含標頭檔案
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //注意: 呼叫 transform 必須包含該標頭檔案
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 節點
ros::init(argc,argv,"tf_sub");
ros::NodeHandle nh;
// 3.建立 TF 訂閱節點
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate r(1);
while (ros::ok())
{
// 4.生成一個座標點(相對於子級座標系)
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "laser";
point_laser.header.stamp = ros::Time::now();
point_laser.point.x = 1;
point_laser.point.y = 2;
point_laser.point.z = 7.3;
// 5.轉換座標點(相對於父級座標系)
//新建一個座標點，用於接收轉換結果
//--------------使用 try 語句或休眠，否則可能由於快取接收延遲而導致座標轉換失敗------------------------
try
{
geometry_msgs::PointStamped point_base;
point_base = buffer.transform(point_laser,"base_link");
ROS_INFO("轉換後的資料:(%.2f,%.2f,%.2f),參考的座標系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("程式異常.....");
}
r.sleep();
ros::spinOnce();
}
return 0;
}
配置檔案此處略。
4.執行
可以使用命令列或launch檔案的方式分別啟動釋出節點與訂閱節點，如果程式無異常，控制檯將輸出，座標轉換後的結果。
方案B:Python實現
1.建立功能包
建立專案功能包依賴於 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
2.釋出方
#! /usr/bin/env python
"""
靜態座標變換髮布方:
釋出關於 laser 座標系的位置資訊
實現流程:
1.導包
2.初始化 ROS 節點
3.建立 靜態座標廣播器
4.建立並組織被廣播的訊息
5.廣播器傳送訊息
6.spin
"""
# 1.導包
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped
if __name__ == "__main__":
# 2.初始化 ROS 節點
rospy.init_node("static_tf_pub_p")
# 3.建立 靜態座標廣播器
broadcaster = tf2_ros.StaticTransformBroadcaster()
# 4.建立並組織被廣播的訊息
tfs = TransformStamped()
# --- 頭資訊
tfs.header.frame_id = "world"
tfs.header.stamp = rospy.Time.now()
tfs.header.seq = 101
# --- 子座標系
tfs.child_frame_id = "radar"
# --- 座標系相對資訊
# ------ 偏移量
tfs.transform.translation.x = 0.2
tfs.transform.translation.y = 0.0
tfs.transform.translation.z = 0.5
# ------ 四元數
qtn = tf.transformations.quaternion_from_euler(0,0,0)
tfs.transform.rotation.x = qtn[0]
tfs.transform.rotation.y = qtn[1]
tfs.transform.rotation.z = qtn[2]
tfs.transform.rotation.w = qtn[3]
# 5.廣播器傳送訊息
broadcaster.sendTransform(tfs)
# 6.spin
rospy.spin()
許可權設定以及配置檔案此處略。
3.訂閱方
#! /usr/bin/env python
"""
訂閱座標系資訊，生成一個相對於 子級座標系的座標點資料，
轉換成父級座標系中的座標點
實現流程:
1.導包
2.初始化 ROS 節點
3.建立 TF 訂閱物件
4.建立一個 radar 座標系中的座標點
5.調研訂閱物件的 API 將 4 中的點座標轉換成相對於 world 的座標
6.spin
"""
# 1.導包
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 內建的訊息型別
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped
if __name__ == "__main__":
# 2.初始化 ROS 節點
rospy.init_node("static_sub_tf_p")
# 3.建立 TF 訂閱物件
buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buffer)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# 4.建立一個 radar 座標系中的座標點
point_source = PointStamped()
point_source.header.frame_id = "radar"
point_source.header.stamp = rospy.Time.now()
point_source.point.x = 10
point_source.point.y = 2
point_source.point.z = 3
try:
# 5.調研訂閱物件的 API 將 4 中的點座標轉換成相對於 world 的座標
point_target = buffer.transform(point_source,"world")
rospy.loginfo("轉換結果:x = %.2f, y = %.2f, z = %.2f",
point_target.point.x,
point_target.point.y,
point_target.point.z)
except Exception as e:
rospy.logerr("異常:%s",e)
# 6.spin
rate.sleep()
許可權設定以及配置檔案此處略。
PS: 在 tf2 的 python 實現中，tf2 已經封裝了一些訊息型別，不可以使用 geometry_msgs.msg 中的型別
4.執行
可以使用命令列或launch檔案的方式分別啟動釋出節點與訂閱節點，如果程式無異常，控制檯將輸出，座標轉換後的結果。
補充1:
當座標系之間的相對位置固定時，那麼所需引數也是固定的: 父系座標名稱、子級座標系名稱、x偏移量、y偏移量、z偏移量、x 翻滾角度、y俯仰角度、z偏航角度，實現邏輯相同，引數不同，那麼 ROS 系統就已經封裝好了專門的節點，使用方式如下:
rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滾角度 父級座標系 子級座標系
示例:rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser
也建議使用該種方式直接實現靜態座標系相對資訊釋出。
補充2:
可以藉助於rviz顯示座標系關係，具體操作:
- 新建視窗輸入命令:rviz;
- 在啟動的 rviz 中設定Fixed Frame 為 base_link;
- 點選左下的 add 按鈕，在彈出的視窗中選擇 TF 元件，即可顯示座標關係。
另請參考:
-
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20static%20broadcaster%20%28C%2B%2B%29
-
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20static%20broadcaster%20%28Python%29
-
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28C%2B%2B%29
-
http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20listener%20%28Python%29