ROS学习笔记8：TF坐标变换

前言

本人ROS小白，利用寒假时间学习ROS，在此以笔记的方式记录自己每天的学习过程。争取写满20篇(8/20)。
环境：Ubuntu20.04、ROS1：noetic
环境配置：严格按照下方学习链接的教程配置，基本一次成功。
学习链接：【Autolabor初级教程】ROS机器人入门
对应链接文档：ROS机器人入门课程《ROS理论与实践》
笔记绝大部分代码使用Python语言编写。
本期关键词：静态变换，动态变换，多坐标变换

准备工作

了解坐标变换中常用的消息载体：geometry_msgs/TransformStamped和geometry_msgs/PointStamped。

geometry_msgs/TransformStamped

1. geometry_msgs/TransformStamped用于传输坐标系相关位置信息
2. 先运行一下ROS自带的乌龟跟随指令：roslaunch turtle_tf2 turtle_tf2_demo.launch。如果运行时报错，可执行：sudo ln -s /usr/bin/python3 /usr/bin/python ，这个命令在Linux系统中创建了一个符号链接（也称为软链接），它将/usr/bin/python链接到/usr/bin/python3。这个操作通常用于解决Python版本冲突或者指定系统默认使用的Python版本。然后新开终端运行rostopic list，得到以下结果：（我们主要关注/tf_static。）

/rosout
/rosout_agg
/tf
/tf_static
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose
/turtle2/cmd_vel
/turtle2/color_sensor
/turtle2/pose

3. 依次执行以下指令：（或者也可以直接执行最后一条指令）

rostopic type /tf_static
rosmsg info tf2_msgs/TFMessage
rosmsg info geometry_msgs/TransformStamped

4. 结果如下：

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/Transform transform
  geometry_msgs/Vector3 translation
    float64 x
    float64 y
    float64 z
  geometry_msgs/Quaternion rotation
    float64 x
    float64 y
    float64 z
    float64 w

5. 需要注意：frame_id（父坐标系）是属于header字段的，child_frame_id（子坐标系）是单独一个字段的，写代码时不要弄混。
6. 消息类型中的translation代表坐标系在xyz轴上的偏移，rotation代表坐标系的翻转、俯仰、偏航状态，表示方法是四元数表示法（不理解四元数也没关系，不影响写代码和理解坐标系的位置状态，如果想深入了解四元数，可参考：[四元数的可视化](【四元数的可视化】 https://www.bilibili.com/video/BV1SW411y7W1/?share_source=copy_web&vd_source=181138744abfbdc4eb6c07c38a129a45)。）

geometry_msgs/PointStamped

1. geometry_msgs/PointStamped用于传输某个坐标系内坐标点的信息
2. 这里我们直接用rosmsg info geometry_msgs/PointStamped命令查看：

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Point point
  float64 x
  float64 y
  float64 z

3. 坐标点的消息类型比坐标系的消息类型简单，各字段参数的含义均相同（但无四元数表示）。

一、静态变换

两个部分：发布方，订阅方

代码编写

1. 发布方

代码实现：

#! usr/bin/env python

import rospy
import tf2_ros
import tf
import tf.transformations
from geometry_msgs.msg import TransformStamped
# geometry_msgs功能包下的msg文件夹，该文件夹下的TransformStamped.msg文件

if __name__=="__main__":
    rospy.init_node("static_tf_pub")
    # 和话题的发布和订阅不同，这里使用广播器广播数据，先创建一个静态坐标广播器
    broadcaster=tf2_ros.StaticTransformBroadcaster()
    # 创建要被广播的消息
    tf_pub=TransformStamped()
    # 开始设置该消息
    tf_pub.header.stamp=rospy.Time.now()
    tf_pub.header.frame_id="base_link"# 父坐标系，也可以设置为world，方便理解即可
    tf_pub.child_frame_id="radar"
    tf_pub.transform.translation.x=0.1
    tf_pub.transform.translation.y=0.2
    tf_pub.transform.translation.z=0.3
    # 以下是四元数的设置，这个我们用tf2内置的算法来求，不用自己手算
    # 如果下面的transformations不能自动补齐，可以通过import tf.transformations解决
    qua=tf.transformations.quaternion_from_euler(0,0,0)
    tf_pub.transform.rotation.x=qua[0]
    tf_pub.transform.rotation.y=qua[1]
    tf_pub.transform.rotation.z=qua[2]
    tf_pub.transform.rotation.w=qua[3]
    
    # 以上，数据已经设置好了，接下来就是广播发布
    broadcaster.sendTransform(tf_pub)
    
    rospy.spin()# 确保广播持续进行

指令测试：

rostopic echo /tf_static 

结果：

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1706501229
        nsecs: 920775890
      frame_id: "base_link"
    child_frame_id: "radar"
    transform: 
      translation: 
        x: 0.1
        y: 0.2
        z: 0.3
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0
---

2. 订阅方

代码实现：

#! usr/bin/env python

import rospy
import tf2_ros
from tf2_geometry_msgs import PointStamped

if __name__=="__main__":
    rospy.init_node("static_tf_sub")
    # 创建订阅对象，这里要用到缓冲区
    buffer1=tf2_ros.Buffer()
    # 缓冲区接收数据，这里监听缓冲区的数据
    listener=tf2_ros.TransformListener(buffer1)
    
    rate=rospy.Rate(1) # 频率设置
    # 注意rospy.is_shutdown()一定要带上小括号，不然会出bug！
    while not rospy.is_shutdown():
        point_source=PointStamped()
        point_source.header.frame_id="radar"
        point_source.header.stamp=rospy.Time.now()
        point_source.point.x=3.0
        point_source.point.y=3.0
        point_source.point.z=3.0
        rospy.loginfo("ouput4")
        
        # 避免订阅失败而导致程序中止
        try:
            point_taget=buffer1.transform(point_source,"base_link")
            rospy.loginfo("the result is: x=%.2f,y=%.2f,z=%.2f",
                          point_taget.point.x,
                          point_taget.point.y,
                          point_taget.point.z)
        except Exception as e:
            rospy.logerr("%s",e)
            
        rate.sleep()

指令实现

静态坐标变换的发布还可以用更简便的方法来实现：

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系

示例：

rosrun tf2_ros static_transform_publisher 5 5 5 0 0 0 /base_link /radar

二、动态变换

这里需要借助乌龟运动控制来演示动态坐标变换。

1. 发布方

#! /usr/bin/env python

import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
import tf.transformations
from geometry_msgs.msg import TransformStamped

def doPose(pose):
    # 创建一个广播器
    broadcaster=tf2_ros.TransformBroadcaster()
    # 创建要被广播的数据
    d_pose=TransformStamped()
    d_pose.header.frame_id="world"
    # 这里要使用rospy.Time.now()，不然用rviz观察时，TF坐标系显示会有bug
    d_pose.header.stamp=rospy.Time.now()
    d_pose.child_frame_id="turtle1"
    d_pose.transform.translation.x=pose.x
    d_pose.transform.translation.y=pose.y
    d_pose.transform.translation.z=0.0
    qua=tf.transformations.quaternion_from_euler(0,0,pose.theta)
    d_pose.transform.rotation.x=qua[0]
    d_pose.transform.rotation.y=qua[1]
    d_pose.transform.rotation.z=qua[2]
    d_pose.transform.rotation.w=qua[3]
    
    broadcaster.sendTransform(d_pose)
    
    
if __name__=="__main__":
    rospy.init_node("dynamic_pub")
    # 我们要根据/turtle1/pose的数据来设置tf坐标系的数据，所以先订阅/turtle1/pose
    sub=rospy.Subscriber("/turtle1/pose",Pose,doPose)
    
    rospy.spin()

这里仍然可以使用rostopic echo指令来查看发布方发布的tf数据。

2. 订阅方

#! usr/bin/env python

import rospy
import tf2_ros
from tf2_geometry_msgs import PointStamped


if __name__=="__main__":
    rospy.init_node("dynamic_sub")
    # 创建订阅对象，这里要用到缓冲区
    buffer1=tf2_ros.Buffer()
    listener=tf2_ros.TransformListener(buffer1)
    
    rate=rospy.Rate(10)
    while not rospy.is_shutdown():
	    # 创建一个坐标点
        point_source=PointStamped()
        # 设置坐标点信息
        point_source.header.frame_id="turtle1"
        
        # 这里不设置时间戳，使用rospy.Time()，防止发布方和接收方时间戳差异过大而报错
        point_source.header.stamp=rospy.Time()
        # 将“turtle1”坐标系下的点设置为原点，方便跟/turtle1/pose比对数据
        point_source.point.x=0
        point_source.point.y=0
        point_source.point.z=0
        try:
            point_taget=buffer1.transform(point_source,"world")
            rospy.loginfo("the result is: x=%.2f,y=%.2f,z=%.2f",
                          point_taget.point.x,
                          point_taget.point.y,
                          point_taget.point.z)
        except Exception as e:
            rospy.logerr("%s",e)
            
        rate.sleep()

三、多坐标变换

1. 发布方

用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>

2. 订阅方

#! usr/bin/env python

import rospy
import tf2_ros
from tf2_geometry_msgs import PointStamped
from geometry_msgs.msg import TransformStamped


if __name__=="__main__":
    rospy.init_node("tf2_more_sub")
    # 创建订阅对象，这里要用到缓冲区
    buffer1=tf2_ros.Buffer()
    listener=tf2_ros.TransformListener(buffer1)

    rate=rospy.Rate(1)
    while not rospy.is_shutdown():
        try:
            # 计算son1相对于son2的坐标关系(son1的值-son2的值)
            # rospy.Time(0)的作用：取时间间隔最近的两个坐标系帧结果
            tfs=buffer1.lookup_transform("son2","son1",rospy.Time(0))
            rospy.loginfo("父坐标系：%s,子坐标系：%s",tfs.header.frame_id,tfs.child_frame_id)            
            rospy.loginfo("the result is: x=%.2f,y=%.2f,z=%.2f",
                          tfs.transform.translation.x,
                          tfs.transform.translation.y,
                          tfs.transform.translation.z)
            
            point_source=PointStamped()
            point_source.header.frame_id="son1"
            point_source.header.stamp=rospy.Time()
            point_source.point.x = 0.1
            point_source.point.y = 0.1
            point_source.point.z = 0.1
            
            point_target = buffer1.transform(point_source,"son2")
            
            rospy.loginfo("point_target 所属的坐标系:%s",point_target.header.frame_id)
            rospy.loginfo("坐标点相对于 son2 的坐标:(%.2f,%.2f,%.2f)",
                        point_target.point.x,
                        point_target.point.y,
                        point_target.point.z
            )
            
        except Exception as e:
            rospy.logerr("%s",e)
            
        rate.sleep()

输出结果为：

父坐标系：son2,子坐标系：son1
the result is: x=-0.30,y=0.00,z=0.00
point_target 所属的坐标系:son2
坐标点相对于 son2 的坐标:(-0.20,0.10,0.10)

往期内容

1. ROS学习笔记1：基础知识
2. ROS学习笔记2：话题通信
3. ROS学习笔记3：服务通信与参数服务器
4. ROS常用命令记录
5. ROS学习笔记4：通信机制实操
6. ROS学习笔记5：常用API和模块导入
7. ROS学习笔记6：launch文件
8. ROS学习笔记7：重名解决与名称设置