[ROS]三种坐标变换代码实现及详解（静态坐标转换，动态坐标转换，多坐标转换）

一：静态坐标转换

发布方

# 静态发布方，发布两个坐标系的相对关系（车辆底盘————base_link(基础坐标系) ，雷达————laser(子坐标系）
import rospy
import tf.transformations
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf

rospy.init_node("static_pub")
pub = tf2_ros.StaticTransformBroadcaster()

# 组织被发布的对象
ts = TransformStamped()
# header
ts.header.stamp = rospy.Time.now()
ts.header.frame_id = "base_link"

# child_frame_id
ts.child_frame_id = "laser"

# 相对关系（偏移，四元数）
ts.transform.translation.x = 0.2
ts.transform.translation.y = 0.0
ts.transform.translation.z = 0.5

# 先从欧拉角转化成四元数
# 因为雷达没有俯仰角，偏航角和翻滚角，就写0，0，0
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(ts)

rospy.spin()

订阅方

import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs

rospy.init_node("static_sub")

# 创建缓存对象
buffer = tf2_ros.Buffer()
# 创建订阅者对象
sub = tf2_ros.TransformListener(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:
        # 参数1：被转换的坐标点，参数2：目标坐标系，返回值：转换后的坐标点
        ps_out = buffer.transform(ps, "base_link")
        # 输出结果
        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 e:
        rospy.logwarn("错误提示:%s", e)
    
    rate.sleep()

二：动态坐标转换

发布方

# 订阅乌龟的位姿信息，转化成坐标系的相对关系
# 话题：/tuetle1/pose
# 类型：/turtlesim/Pose
import rospy
import tf.transformations
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf

# 回调函数处理订阅到的消息
def doPose(pose):
    # 创建发布坐标系相对关系的对象
    pub = tf2_ros.TransformBroadcaster()
    # 将pose转化成坐标系相对位置关系
    ts = TransformStamped()
    ts.header.frame_id = "world"
    ts.header.stamp = rospy.Time.now()

    ts.child_frame_id = "turtle1"

    ts.transform.translation.x = pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0

    # 从欧拉角转化四元数，乌龟只有偏航角，获取的话是0，0， pose.theta
    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)

rospy.init_node("dynamic_pub")

sub = rospy.Subscriber("/turtle1/pose", Pose, doPose, queue_size=100)

rospy.spin()

订阅方（与静态坐标转换相比，需要改变时间戳）

import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs

rospy.init_node("static_sub")

# 创建缓存对象
buffer = tf2_ros.Buffer()
# 创建订阅者对象
sub = tf2_ros.TransformListener(buffer)

# 组织被转换的坐标点(雷达坐标系中的点)
ps = tf2_geometry_msgs.PointStamped()
# 注意这个地方要把时间戳设置为0
ps.header.stamp = rospy.Time()
ps.header.frame_id = "turtle1"
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:
        # 参数1：被转换的坐标点，参数2：目标坐标系，返回值：转换后的坐标点
        ps_out = buffer.transform(ps, "world")
        # 输出结果
        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 e:
        rospy.logwarn("错误提示:%s", e)
    
    rate.sleep()

三：多坐标转换

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

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

rospy.init_node("static_sub")

# 创建缓存对象
buffer = tf2_ros.Buffer()
# 创建订阅者对象
sub = tf2_ros.TransformListener(buffer)

# 组织被转换的坐标点(雷达坐标系中的点)
ps = tf2_geometry_msgs.PointStamped()
ps.header.stamp = rospy.Time.now()
ps.header.frame_id = "son1"
ps.point.x = 1.0
ps.point.y = 2.0
ps.point.z = 3.0

# 转化逻辑实现
rate = rospy.Rate(10)
while not rospy.is_shutdown():
    try:
        # 计算son1相对于son2的坐标关系
        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
                      )
        # 参数1：被转换的坐标点，参数2：目标坐标系，返回值：转换后的坐标点
        ps_out = buffer.transform(ps, "son2")
        # 输出结果
        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 e:
        rospy.logwarn("错误提示:%s", e)
    
    rate.sleep()