ROS2 tf2坐标变换发布监听解读

最新推荐文章于 2025-01-27 01:41:57 发布

fegxg

最新推荐文章于 2025-01-27 01:41:57 发布

阅读量1.1k

点赞数 15

文章标签： python linux 机器人

本文链接：https://blog.youkuaiyun.com/fegxg/article/details/144515516

版权

一、tf坐标变换广播

1.广播坐标系变换

可以使用tf的坐标广播工具进行广播坐标关系，广播时需要三个数据：
父坐标系名称（字符串）
子坐标系名称（字符串）
父子之间的变换关系（平移关系和旋转关系）

这里变换关系需要位置和姿态的变换关系，位置的描述使用的是xyz三个参数，而姿态的描述则分两种：

第一种是四元数形式（qx qy qz qw）。
第二种是欧拉角形式（yaw偏航角-rz pitch俯仰角-ry roll滚转角-rx），注意角度单位采用弧度制。

命令行：

ros2 run tf2_ros static_transform_publisher 0 0 3 0 0 3.14 B C

使用ros2 run运行tf2_ros功能包，调用工具static_transform_publisher来发布变换信息，

0 0 3是位置变换xyz的坐标，这里姿态变换采用了欧拉角的形式（弧度制），角度为0 0 3.14即绕x轴转180度，变换为B坐标系到C坐标系。

2.监听坐标系变换

ros2 run tf2_ros tf2_echo B C

使用ros2 run运行tf2_tos功能包里的工具tf2_echo来监听B到C的坐标系变换。终端中不断输出B和C之间的平移和旋转，平移采用的是xyz，旋转部分采用的是四元数表示。

二、坐标变换发布监听Python实现

1.坐标变换广播

坐标变换分为两种：

坐标系之间的关系不随时间推移而改变，称为静态坐标变换，需要使用静态广播发布器。如两个固定件之间的坐标变换。

坐标系之间的关系随时间的推移而改变，称为（动态）坐标变换，使用广播发布器（TransformBroadcaster）发布坐标关系。如一个部件在运动，一个部件固定。

1.静态坐标变换

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
from tf2_ros import StaticTransformBroadcaster
rclpy.init()
node = Node("transform_node")
static_tf_pub = StaticTransformBroadcaster(node)
t = TransformStamped()
t.header.stamp = node.get_clock().now().to_msg()
t.header.frame_id = 'B'  # 父坐标系
t.child_frame_id = 'C'   # 子坐标系

t.transform.translation.x = 0.0
t.transform.translation.y = 0.0
t.transform.translation.z = 3.0
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 1.0
t.transform.rotation.w = 0.0

static_tf_pub.sendTransform(t)

通过静态坐标变换广播器 (StaticTransformBroadcaster) 发布一个固定的坐标系变换关系。

首先使用.init()方法初始化节点，创建一个名为 transform_node 的节点。

static_tf_pub = StaticTransformBroadcaster(node)

之后创建一个StaticTransformBroadcaster静态坐标广播器static_tf_pub，传入节点node，将该节点与 TF2 系统连接，负责发布静态变换数据。

创建一个TransformStamped()tf帧的实例t。

我们首先来查看消息接口tf帧的内容，tf变换的消息接口是geometry_msgs/msg/TransformStamped，使用命令行查看。

ros2 interface show geometry_msgs/msg/TransformStamped

之后可以看到消息接口的信息：

# 消息头，包括坐标系的时间戳和父坐标系名
std_msgs/Header header
string child_frame_id  # 子坐标系名
geometry_msgs/Transform transform  # 变换数据（平移+旋转）

这是一个混合类型的接口数据，继续使用上方命令查看每一个部分的内容，可以得到整个消息接口类型：

# 头部信息，包括时间戳和父坐标系名称
std_msgs/Header header
  uint32 seq
  builtin_interfaces/Time stamp
    int32 sec
    uint32 nanosec
  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

这里包含父坐标系名称 frame_id，子坐标系名称child_frame_id，时间戳 stamp（秒和纳秒），

translation平移向量，表示坐标变换的平移部分 (x, y, z)，rotation四元数，表示坐标变换的旋转部分 (x, y, z, w)。

t.header.stamp = node.get_clock().now().to_msg()
t.header.frame_id = 'B'  # 父坐标系
t.child_frame_id = 'C'   # 子坐标系

设置父坐标系为B，子坐标系为C。

t.header.stamp = node.get_clock().now().to_msg()是设置时间戳。具体为调用node节点的方法get_clock()来获取当前ROS2系统时间，now() 方法返回当前的时间信息，数据类型是 rclpy.time.Time 对象。to_msg()方法是将对象转换为 ROS2 消息格式，即 builtin_interfaces.msg.Time，这是 ROS2 中标准化的时间戳格式，用于消息头部 (Header) 的时间标记。然后将值赋给t.head.stamp，用于标记这条坐标变换信息是在哪个时刻生成的。

t.transform.translation.x = 0.0
t.transform.translation.y = 0.0
t.transform.translation.z = 3.0
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 1.0
t.transform.rotation.w = 0.0

然后分别设置translation和rotation的内容。

然后我们把该数据帧t广播到 TF2 系统

static_tf_pub.sendTransform(t)

发布之后，其他节点就可以监听该坐标变换。

2.动态坐标变换

import rclpy  # 导入ROS 2 Python库
from rclpy.node import Node  # 导入Node类，用于创建节点
from geometry_msgs.msg import TransformStamped  # 导入TF变换的消息接口
from tf2_ros import TransformBroadcaster  # 从tf2_ros导入变换广播器
rclpy.init()
node = Node("transform_node2")
tf_pub = TransformBroadcaster(node)
t = TransformStamped()
t.header.stamp = node.get_clock().now().to_msg()  # 设置时间戳
t.header.frame_id = 'C'  # 父坐标系frame（parent frame）
t.child_frame_id = 'P'   # 子坐标系frame（child frame）
t.transform.translation.x = 2.0
t.transform.translation.y = 1.0
t.transform.translation.z = 0.0

t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 0.0
t.transform.rotation.w = 1.0

def send_transform():
    t.header.stamp = node.get_clock().now().to_msg()  # 更新时间戳
    tf_pub.sendTransform(t)  # 发送变换消息

node.create_timer(0.1, send_transform)  # 0.1秒调用一次send_transform函数
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()

首先初始化节点并创立一个节点，命名为"transform_node2"。

tf_pub = TransformBroadcaster(node)

这里创建一个TF坐标发布器，传入节点node，将该节点与 TF2 系统连接，负责发布坐标变换数据。

然后创建一个tf帧TransformBroadcaster的实例t，按照我们上面查看的消息接口的内容，逐步设置其父坐标系名称 frame_id，子坐标系名称child_frame_id，时间戳 stamp（秒和纳秒），translation平移向量，rotation四元数。

t.header.stamp = node.get_clock().now().to_msg()  # 设置时间戳
t.header.frame_id = 'C'  # 父坐标系frame（parent frame）
t.child_frame_id = 'P'   # 子坐标系frame（child frame）
t.transform.translation.x = 2.0
t.transform.translation.y = 1.0
t.transform.translation.z = 0.0

t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 0.0
t.transform.rotation.w = 1.0

因为上面的接口数据类型可以看到父坐标系名frame_id在header下面，所以父坐标系名的设立在header.frame_id。

node.create_timer(0.1, send_transform)

这里调用节点方法create_timer来创建一个定时器，第一个参数传入周期0.1秒，第二个参数传入回调函数send_transform。

def send_transform():
    t.header.stamp = node.get_clock().now().to_msg()  # 更新时间戳
    tf_pub.sendTransform(t)  # 发送变换消息

进入回调函数，首先更新时间戳为当前的时间，然后发送我们的tf帧t，这样做到了每0.1秒发送一个不断变化的坐标系变换。

最后使节点不断运行和最终停止节点销毁节点。

3.坐标变换监听

import rclpy
from rclpy.node import Node
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
rclpy.init()
node = Node("transform_node3")
tf_buffer = Buffer()
tf_listener = TransformListener(tf_buffer, node)
def transform_callback():
    try:
        now = rclpy.time.Time()
        trans = tf_buffer.lookup_transform('B', 'P', now)
        print("trans", trans)
    except TransformException as ex:
        print(f'Could not transform B to P: {ex}')
node.create_timer(1, transform_callback)
rclpy.spin(node)

首先节点初始化并创立一个节点，命名为"transform_node3"。

tf_buffer = Buffer()
tf_listener = TransformListener(tf_buffer, node)

这里创建一个Buffer()对象tf_buffer，该对象用来存储和缓存接收到的坐标变换数据。

然后创建一个TransformListener对象tf_listener，该对象会监听和接收变换数据，并将这些数据存储到tf_buffer中。传入的第一个参数是tf_buffer用于缓存，第二个参数是node，将该节点与 TF2 系统连接，负责监听坐标变换数据。

node.create_timer(1, transform_callback)

调用node节点的方法create_timer来创建一个定时器，第一个传入参数的定时器周期1秒，第二个传入参数是定时器回调函数transform_callback。

def transform_callback():
    try:
        now = rclpy.time.Time()
        trans = tf_buffer.lookup_transform('B', 'P', now)
        print("trans", trans)
    except TransformException as ex:
        print(f'Could not transform B to P: {ex}')

进入回调函数，首先获取当前时间now，因为我们的tf帧含时间戳，每一次坐标变换数据都有一个时间对应。

trans = tf_buffer.lookup_transform('B', 'P', now)

tf_buffer缓存中去查询从坐标系B到坐标系P的变换数据，返回所需要的变换并保存到trans中，然后我们打印变换信息。

如果无法获取到变换数据（如找不到坐标系B或P），则会抛出TransformException异常，捕获后打印错误信息。