Halo，這里是Ppeua。平時主要更新C++，數據結構算法…感興趣就關注我吧！你定不會失望。
文章目錄
- 0.ROS中的坐標轉換消息包
- 0.1 geometry_msgs/TransformStamped
- 0.2 geometry_msgs/PointStamped
- 1.靜態坐標轉換
- 1.1導入所需功能包
- 1.2發布方實現
- 1.3 訂閱方實現
- 1.4 tf2_ros實現靜態坐標轉換
- 2.動態坐標轉換
- 2.1發布方實現
- 2.2訂閱方邏輯
- 2.3實現效果
- 3.0多坐標轉換
- 3.1發布方實現
- 3.2訂閱方實現
- 3.3 view_frames查看當前坐標系
- 4.0 tf坐標變換實操
- 4.1烏龜位姿信息發布
- 4.2 控制烏龜進行跟隨運動
- 4.3查看當前坐標關系
0.ROS中的坐標轉換消息包
在日常生活中，特別是對于機器人來說，各個目標系中的坐標轉換是很關鍵的，通過右手系來標注坐標。
ROS中提供了坐標轉換的軟件包 Transform Frame TF的作用是ROS中實現不同坐標點/向量的轉換。
不過TF在若干個版本前已經棄用，現在使用的是全新的版本：TF2，其有幾個相關的功能包：
tf2_geometry_msgs:可以將ROS消息轉換成tf2消息。tf2: 封裝了坐標變換的常用消息。tf2_ros:為tf2提供了roscpp和rospy綁定，封裝了坐標變換常用的API。
在坐標系轉換中，在geometry下有兩個重要的消息類型：TransformStamped、PointStamped,前者用于坐標系間的轉換，后者用于點之間的坐標轉換，這對我們之后的使用很重要。先來了解下這兩種消息類型中的內容。
0.1 geometry_msgs/TransformStamped
該消息類型表示坐標系之間的關系
在終端中輸入
rosmsg info geometry_msgs/TransformStamped
查看該消息類型的具體信息：
std_msgs/Header header # 頭信息uint32 seq ## 序列號 time stamp ## 時間戳string frame_id ## 坐標string child_frame_id # 子坐標geometry_msgs/Transform transform #坐標信息geometry_msgs/Vector3 translation ##偏移量float64 xfloat64 yfloat64 zgeometry_msgs/Quaternion rotation #四元數(歐拉角)float64 xfloat64 yfloat64 zfloat64 w
可以看出 PointStamped消息是由:
std_msgs/Header,string,geometry_msgs/Transform封裝在一起，組成的新消息類型。
其中Transform又是由geometry_msgs/Vector3,geometry_msgs/Quaternion進行封裝的。
0.2 geometry_msgs/PointStamped
該消息類型表示坐標點之間的轉換
在終端中輸入
rosmsg info geometry_msgs/PointStamped
可以查看該消息中的具體信息
std_msgs/Header header #頭信息uint32 seq ##序列號time stamp ##時間戳string frame_id ##坐標系
geometry_msgs/Point point #點坐標float64 x float64 yfloat64 z
可以看出 PointStamped消息是由:
std_msgs/Header與geometry_msgs/Point封裝在一起，組成的新消息類型。
1.靜態坐標轉換
現有一機器人模型，核心構成包含主體與雷達，各對應一坐標系，坐標系的原點分別位于主體與雷達的物理中心，已知雷達原點相對于主體原點位移關系如下: x 0.2 y0.0 z0.5。當前雷達檢測到一障礙物，在雷達坐標系中障礙物的坐標為 (2.0 3.0 5.0),請問，該障礙物相對于主體的坐標是多少？
組織下我們發布方的整體邏輯:
導入所需功能包初始化ros節點創建靜態坐標廣播器編寫靜態坐標信息發送消息spin()
這里是接收方的邏輯：
導入所需要的功能包初始化ros節點創建TF訂閱對象創建lase的坐標點坐標轉換spin（）
1.1導入所需功能包
在這個案例中，需要:rospy,std_msgs 這兩個標準件
還需要：
tf2:封裝了坐標變換的常用消息。
tf2_ros: 為tf2提供了roscpp和rospy綁定，封裝了坐標變換常用的API。
tf2_geometry_msgs:可以將ROS消息轉換成tf2消息。
1.2發布方實現
"""
導入功能包
"""
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
import rospy
"""
初始化節點信息
創建發布對象
組織發布數據
發布數據
spin()
"""
#初始化ros節點
rospy.init_node("static_pub")
#創建靜態發布對象
pub=tf2_ros.StaticTransformBroadcaster()
#組織消息類型
ts=TransformStamped()ts.header.seq=123
ts.header.stamp=rospy.Time.now()
ts.child_frame_id="laser"
ts.header.frame_id="frame_id"
ts.transform.translation.x=0.2
ts.transform.translation.y=0
ts.transform.translation.z=0.5
"""
將歐拉角放到四元數中進行轉換
用到了tf中的transformation.quaternion_from_euler
"""
qtn=tf.transformations.quaternion_from_euler(0,0,0)
ts.transform.rotation.x=qtn[0]
ts.transform.rotation.y=qtn[1]
ts.transform.rotation.z=qtn[2]
ts.transform.rotation.w=qtn[3]
#發布消息
pub.sendTransform(tf)
rospy.spin()
1.3 訂閱方實現
"""
導入功能包
"""
import rospy
from tf2_geometry_msgs import tf2_geometry_msgs
import tf2_ros#初始化節點
rospy.init_node("static_sub")#創建緩存對象
buffer=tf2_ros.Buffer()
"""調用tf2_ros.Buffer()創建一個buffer用來存儲坐標消息"""
tf2_ros.TransformListener(buffer)
"""監聽tf坐標變換,將值存入buffer中""""""創建點坐標信息"""
ps=tf2_geometry_msgs.PointStamped()
ps.header.stamp=rospy.Time.now()
ps.header.frame_id="laser"
ps.point.x=2.0
ps.point.y=3.0
ps.point.z=5.0
rate=rospy.Rate(10)
while not rospy.is_shutdown():try:"""調用buffer.transform 將點坐標與原始坐標進行轉換"""ps_out=buffer.transform(ps,"frame_id")rospy.loginfo("轉換后的坐標：（%.2f,%.2f,%.2f）,參考坐標系：%s",ps_out.point.x,ps_out.point.y,ps_out.point.z,ps_out.header.frame_id)except Exception as ee:rospy.logwarn("錯誤提示%s",ee)rate.sleep()
1.4 tf2_ros實現靜態坐標轉換
由于靜態坐標轉換中的整體邏輯大致相同,所以tf2_ros提供了一個功能包來直接實現坐標轉換,不需要每次都使用編寫代碼
rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滾角度 父級坐標系 子級坐標系
2.動態坐標轉換
在現實生活中，我們面對的不僅有點對點的坐標轉換，還動態的坐標轉換。
我們以烏龜為例來實現一下動態坐標轉換
先來組織下發布方的邏輯
導包 rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim初始化ros節點訂閱 /turtle1/pose 話題消息回調函數 創建TF廣播器組織廣播數據廣播器發布數據 spin
接收方的邏輯導包初始化ros節點創建TF對象處理訂閱數據
2.1發布方實現
import rospy
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
"""訂閱烏龜的位姿信息"""
def doPose(pose):#創建動態坐標發布對象pub=tf2_ros.TransformBroadcaster()#組織點坐標消息類型ts=TransformStamped()ts.header.frame_id="world"ts.child_frame_id="turtle1"ts.header.stamp=rospy.Time.now()#坐標系相對于子集坐標系ts.transform.translation.x=pose.xts.transform.translation.y=pose.yts.transform.translation.z=0#四元數轉換qtn=tf.transformations.quaternion_from_euler(0,0,pose.theta)ts.transform.rotation.x=qtn[0]ts.transform.rotation.y=qtn[1]ts.transform.rotation.z=qtn[2]ts.transform.rotation.w=qtn[3]pub.sendTransform(ts)#初始化ROS節點
rospy.init_node("tf02_pub")
#訂閱消息位姿信息，創建回調函數
sub=rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=100)
rospy.spin()
2.2訂閱方邏輯
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 內置的消息類型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStampedif __name__ == "__main__":# 2.初始化 ROS 節點rospy.init_node("static_sub_tf_p")# 3.創建 TF 訂閱對象buffer = tf2_ros.Buffer()# 監聽坐標變換存入buffer中tf2_ros.TransformListener(buffer)rate = rospy.Rate(1)while not rospy.is_shutdown(): # 4.創建坐標點信息# 僅需提供目標坐標系point_source = PointStamped()point_source.header.frame_id = "turtle1"point_source.header.stamp = rospy.Time.now()try:# 5.調研訂閱對象的 API 將 4 中的點坐標轉換成相對于 world 的坐標point_target = buffer.transform(point_source,"world",rospy.Duration(1))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.spinrate.sleep()
2.3實現效果
首先啟動turtlesim的鍵盤控制節點與GUI
rosrun turtlesim turtlesim_node
rosrun turtlesim turtle_teleop_key
接著啟動發布方與接收方 之后就可以在屏幕上看到轉換后的坐標系
rosrun tf02_dynamic demo01_tf02_pub.py
rosrun tf02_dynamic demo01_tf02_sub.py
3.0多坐標轉換
將多個坐標先相對于世界坐標系進行轉換,然后在調用api將轉換后的數據進行相互轉換
3.1發布方實現
直接調用靜態坐標轉換的ros包,寫成launch文件
<launch><node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" /><node pkg="tf2_ros" type="static_transform_publisher" name="son2"args="0.5 0 0 0 0 0 /world /son2"output="screen"/>
</launch>
3.2訂閱方實現
訂閱方邏輯實現
導入包 rospy std_msgs tf2_ros geometry_msgs(TransformStamped 坐標系轉換) tf2_geomerty_msgs(PointStamped 坐標點轉換)初始化ros節點創建TF訂閱對象,實現兩個坐標系之間相互轉換
import rospy
from tf2_geometry_msgs import tf2_geometry_msgs
import tf2_ros
from geometry_msgs.msg import TransformStampedrospy.init_node("static_sub")#創建緩存對象
buffer=tf2_ros.Buffer()sub=tf2_ros.TransformListener(buffer)rate=rospy.Rate(10)while not rospy.is_shutdown():try:"""計算son1相對于son2的坐標關系lookup_transform(父級坐標系，子級坐標系，取坐標的時間，時間間隔)"""ts=buffer.lookup_transform("son2","son1",rospy.Time(0))rospy.loginfo("父級坐標系：%s，子級坐標系：%s,%.2f,%.2f,%.2f",ts.header.frame_id,ts.child_frame_id,ts.transform.translation.x,ts.transform.translation.y,ts.transform.translation.z)except Exception as ee:passrospy.logwarn("錯誤提示%s",ee)rate.sleep()
3.3 view_frames查看當前坐標系
運行以上節點后，在任意工作目錄下輸入
rosrun tf2_tools view_frames.py
會在當前目錄下生成一個可以坐標關系的pdf，可以利用此工具查看坐標關系
請添加圖片描述
4.0 tf坐標變換實操
我們先來創建turtle，運行turtlesim這個節點
rosrun turtlesim turtlesim_node
通過rosservice的/spawn服務來多生成一只turtle來完成我們的多坐標轉換,生成一只名為H的烏龜
rosservice call /spawn
"x: 0.0
y: 0.0
theta: 0.0
name: ''"
若返回輸入的名字，此時就能在屏幕上看到剛剛生成的那只烏龜
準備工作都做完了，現在開始創建坐標系
4.1烏龜位姿信息發布
先來理清整個跟隨的邏輯：
在坐標系中發布兩只烏龜的信息將第二只烏龜的位姿信息相對第一只烏龜作轉換控制cmd發布速度信息
import rospy
import sys
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tfdef doPose(pose):pub=tf2_ros.TransformBroadcaster()ts=TransformStamped()ts.header.frame_id="world"ts.header.stamp=rospy.Time.now()ts.child_frame_id=turtle_namets.transform.translation.x=pose.xts.transform.translation.y=pose.yqtn=tf.transformations.quaternion_from_euler(0,0,pose.theta)ts.transform.rotation.x=qtn[0]ts.transform.rotation.y=qtn[1]ts.transform.rotation.z=qtn[2]ts.transform.rotation.w=qtn[3]pub.sendTransform(ts)rospy.init_node("dynamic_pub",anonymous=True)
if len(sys.argv)>=2: turtle_name=sys.argv[1]sub=rospy.Subscriber(turtle_name+"/pose",Pose,doPose,queue_size=10)rospy.spin()
else:print(sys.argv[1])rospy.loginfo("請輸入坐標名稱")sys.exit()
這份代碼出現過很多次了，這里就不過多贅述。注意：sys.argv的第一個參數為文件名 之后的為傳入參數
4.2 控制烏龜進行跟隨運動
總體邏輯：
計算兩個烏龜之間的相對坐標控制烏龜的線速度與角速度發布
import rospy
from tf2_geometry_msgs import tf2_geometry_msgs
import tf2_ros
from geometry_msgs.msg import TransformStamped,Twist
import math
import sys
"""
創建訂閱對象
組織被轉換的坐標點
轉換邏輯實現調用tf封裝的算法
輸出結果
"""
rospy.init_node("static_sub")#創建緩存對象
buffer=tf2_ros.Buffer()sub=tf2_ros.TransformListener(buffer)
pub=rospy.Publisher("/H/cmd_vel",Twist,queue_size=10)
rate=rospy.Rate(10)
while not rospy.is_shutdown():try:"""計算son1相對于son2的坐標關系直接監聽整個坐標系，不需要訂閱話題"""ts=buffer.lookup_transform("H","turtle1",rospy.Time(0))rospy.loginfo("父級坐標系：%s，子級坐標系：%s,%.2f,%.2f,%.2f",ts.header.frame_id,ts.child_frame_id,ts.transform.translation.x,ts.transform.translation.y,ts.transform.translation.z)twist=Twist()twist.linear.x=0.5*math.sqrt(math.pow(ts.transform.translation.x,2)+math.pow(ts.transform.translation.y,2))twist.angular.z=4*math.atan2(ts.transform.translation.y,ts.transform.translation.x)pub.publish(twist)except Exception as ee:passrospy.logwarn("錯誤提示%s",ee)rate.sleep()
4.3查看當前坐標關系
rosrun tf2_tools view_frames.py
