【ROS1从入门到精通】第9篇：TF坐标变换（机器人的空间认知）
🎯 本文目标：深入理解ROS TF(Transform)系统，掌握坐标变换的原理和实现，学会管理机器人的坐标系树，能够处理复杂的空间变换问题，实现机器人的精确定位和导航。
📑 目录
1. TF系统概述
1.1 什么是TF？
TF（Transform）是ROS中管理坐标系变换关系的系统。它维护了一个随时间变化的坐标系树，使得我们能够跟踪多个坐标系之间的关系，并在任意两个坐标系之间进行坐标变换。
TF坐标系树 map - 地图坐标系 odom - 里程计坐标系 base_link - 机器人基座 laser_link - 激光雷达 camera_link - 相机 imu_link - IMU wheel_left_link - 左轮 wheel_right_link - 右轮 arm_base_link - 机械臂基座 arm_link_1 arm_link_2 gripper_link - 夹爪
1.2 TF系统特点
| 特性 | 描述 |
|---|---|
| 分布式 | 变换可以由不同节点发布 |
| 时间缓冲 | 保存历史变换信息 |
| 插值 | 自动插值计算中间时刻的变换 |
| 树结构 | 确保无环的层次结构 |
| 双向查询 | 可以查询任意两个坐标系间的变换 |
| 可视化 | 支持RViz等工具的可视化 |
1.3 TF vs TF2
TF2是TF的改进版本，主要区别：
| 特性 | TF | TF2 |
|---|---|---|
| API设计 | 较旧，有些混乱 | 更清晰、模块化 |
| 性能 | 一般 | 更高效 |
| 多机器人 | 支持有限 | 完整支持 |
| 静态变换 | 需要节点持续发布 | 独立的static_transform |
| 缓冲区 | 全局缓冲 | 每个监听器独立缓冲 |
| Python支持 | 完整 | 完整且更高效 |
2. 坐标系基础理论
2.1 坐标变换数学基础
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import numpy as np
import tf
import tf2_ros
from geometry_msgs.msg import Quaternion, Vector3, Transform
class TransformMath:
"""坐标变换数学基础"""
def __init__(self):
self.demonstrate_transform_math()
def demonstrate_transform_math(self):
"""演示坐标变换数学"""
# 1. 欧拉角与四元数转换
self.euler_quaternion_conversion()
# 2. 旋转矩阵
self.rotation_matrix_demo()
# 3. 齐次变换矩阵
self.homogeneous_transform()
# 4. 坐标变换链
self.transform_chain()
def euler_quaternion_conversion(self):
"""欧拉角与四元数转换"""
print("=== Euler to Quaternion Conversion ===")
# 欧拉角 (roll, pitch, yaw) - 弧度
roll = 0.1 # 绕X轴
pitch = 0.2 # 绕Y轴
yaw = 0.3 # 绕Z轴
# 转换为四元数
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
print(f"Euler angles: roll={roll:.2f}, pitch={pitch:.2f}, yaw={yaw:.2f}")
print(f"Quaternion: x={quaternion[0]:.3f}, y={quaternion[1]:.3f}, "
f"z={quaternion[2]:.3f}, w={quaternion[3]:.3f}")
# 四元数转回欧拉角
euler = tf.transformations.euler_from_quaternion(quaternion)
print(f"Back to Euler: roll={euler[0]:.2f}, pitch={euler[1]:.2f}, "
f"yaw={euler[2]:.2f}")
# 四元数归一化（重要！）
norm = np.linalg.norm(quaternion)
quaternion_normalized = quaternion / norm
print(f"Normalized quaternion norm: {np.linalg.norm(quaternion_normalized):.6f}")
def rotation_matrix_demo(self):
"""旋转矩阵演示"""
print("\n=== Rotation Matrix ===")
# 从欧拉角创建旋转矩阵
roll, pitch, yaw = 0.1, 0.2, 0.3
R = tf.transformations.euler_matrix(roll, pitch, yaw)
print(f"Rotation matrix from Euler angles:")
print(R[:3, :3])
# 从旋转矩阵提取欧拉角
euler = tf.transformations.euler_from_matrix(R)
print(f"Extracted Euler: {euler}")
# 绕轴旋转
axis = [0, 0, 1] # Z轴
angle = np.pi / 4 # 45度
R_axis = tf.transformations.rotation_matrix(angle, axis)
print(f"Rotation matrix around Z-axis by 45°:")
print(R_axis[:3, :3])
def homogeneous_transform(self):
"""齐次变换矩阵"""
print("\n=== Homogeneous Transform ===")
# 创建变换矩阵
# 平移
translation = [1.0, 2.0, 3.0]
# 旋转（四元数）
quaternion = tf.transformations.quaternion_from_euler(0, 0, np.pi/4)
# 组合成齐次变换矩阵
T = tf.transformations.quaternion_matrix(quaternion)
T[0:3, 3] = translation
print(f"Homogeneous transform matrix:")
print(T)
# 应用变换
point = [1, 0, 0, 1] # 齐次坐标
transformed_point = T @ point
print(f"Original point: {point[:3]}")
print(f"Transformed point: {transformed_point[:3]}")
# 逆变换
T_inv = tf.transformations.inverse_matrix(T)
point_back = T_inv @ transformed_point
print(f"Inverse transformed: {point_back[:3]}")
def transform_chain(self):
"""坐标变换链"""
print("\n=== Transform Chain ===")
# A到B的变换
T_AB = tf.transformations.euler_matrix(0, 0, np.pi/4)
T_AB[0:3, 3] = [1, 0, 0]
# B到C的变换
T_BC = tf.transformations.euler_matrix(0, 0, np.pi/4)
T_BC[0:3, 3] = [0, 1, 0]
# A到C的变换 = A到B * B到C
T_AC = T_AB @ T_BC
print("Transform A->B->C:")
print(f"Translation: {T_AC[0:3, 3]}")
# 从变换矩阵提取平移和旋转
translation = T_AC[0:3, 3]
quaternion = tf.transformations.quaternion_from_matrix(T_AC)
euler = tf.transformations.euler_from_matrix(T_AC)
print(f"Final translation: {translation}")
print(f"Final rotation (euler): {euler}")2.2 坐标系约定
python
class CoordinateConventions:
"""ROS坐标系约定"""
def __init__(self):
"""
ROS使用右手坐标系：
- X轴：向前（红色）
- Y轴：向左（绿色）
- Z轴：向上（蓝色）
旋转约定：
- Roll：绕X轴旋转
- Pitch：绕Y轴旋转
- Yaw：绕Z轴旋转
"""
self.standard_frames = {
'map': '全局固定坐标系，通常是建图的起点',
'odom': '里程计坐标系，连续但会漂移',
'base_link': '机器人本体坐标系',
'base_footprint': '机器人在地面的投影',
'laser_link': '激光雷达坐标系',
'camera_link': '相机坐标系',
'imu_link': 'IMU坐标系',
'gps_link': 'GPS坐标系'
}
def print_conventions(self):
"""打印坐标系约定"""
print("=== ROS Coordinate Conventions ===")
print("Right-handed coordinate system:")
print(" X-axis: Forward (Red)")
print(" Y-axis: Left (Green)")
print(" Z-axis: Up (Blue)")
print("\nStandard frames:")
for frame, description in self.standard_frames.items():
print(f" {frame}: {description}")3. 静态坐标变换
3.1 静态变换发布器
cpp
// static_transform_publisher.cpp
#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>
class StaticTransformPublisher {
public:
StaticTransformPublisher() {
// 发布静态变换
publishStaticTransforms();
}
void publishStaticTransforms() {
// 创建静态变换广播器
static tf2_ros::StaticTransformBroadcaster static_broadcaster;
// 1. base_link到laser_link的变换
geometry_msgs::TransformStamped laser_transform;
laser_transform.header.stamp = ros::Time::now();
laser_transform.header.frame_id = "base_link";
laser_transform.child_frame_id = "laser_link";
// 设置平移（激光雷达在机器人前方0.2m，高0.1m）
laser_transform.transform.translation.x = 0.2;
laser_transform.transform.translation.y = 0.0;
laser_transform.transform.translation.z = 0.1;
// 设置旋转（无旋转）
tf2::Quaternion q;
q.setRPY(0, 0, 0);
laser_transform.transform.rotation.x = q.x();
laser_transform.transform.rotation.y = q.y();
laser_transform.transform.rotation.z = q.z();
laser_transform.transform.rotation.w = q.w();
// 2. base_link到camera_link的变换
geometry_msgs::TransformStamped camera_transform;
camera_transform.header.stamp = ros::Time::now();
camera_transform.header.frame_id = "base_link";
camera_transform.child_frame_id = "camera_link";
// 相机在机器人前方0.15m，高0.3m，向下倾斜15度
camera_transform.transform.translation.x = 0.15;
camera_transform.transform.translation.y = 0.0;
camera_transform.transform.translation.z = 0.3;
q.setRPY(0, -0.2618, 0); // pitch = -15度
camera_transform.transform.rotation.x = q.x();
camera_transform.transform.rotation.y = q.y();
camera_transform.transform.rotation.z = q.z();
camera_transform.transform.rotation.w = q.w();
// 3. base_link到imu_link的变换
geometry_msgs::TransformStamped imu_transform;
imu_transform.header.stamp = ros::Time::now();
imu_transform.header.frame_id = "base_link";
imu_transform.child_frame_id = "imu_link";
imu_transform.transform.translation.x = 0.0;
imu_transform.transform.translation.y = 0.0;
imu_transform.transform.translation.z = 0.05;
// IMU可能有安装角度偏差
q.setRPY(0, 0, 0);
imu_transform.transform.rotation.x = q.x();
imu_transform.transform.rotation.y = q.y();
imu_transform.transform.rotation.z = q.z();
imu_transform.transform.rotation.w = q.w();
// 发布所有静态变换
std::vector<geometry_msgs::TransformStamped> static_transforms;
static_transforms.push_back(laser_transform);
static_transforms.push_back(camera_transform);
static_transforms.push_back(imu_transform);
static_broadcaster.sendTransform(static_transforms);
ROS_INFO("Published static transforms");
}
private:
ros::NodeHandle nh_;
};
int main(int argc, char** argv) {
ros::init(argc, argv, "static_transform_publisher");
StaticTransformPublisher publisher;
ros::spin();
return 0;
}3.2 Python静态变换发布
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf.transformations as tf_trans
class StaticTransformPublisher:
"""静态坐标变换发布器"""
def __init__(self):
rospy.init_node('static_transform_publisher')
# 创建静态变换广播器
self.static_broadcaster = tf2_ros.StaticTransformBroadcaster()
# 发布静态变换
self.publish_static_transforms()
def publish_static_transforms(self):
"""发布静态变换"""
static_transforms = []
# 1. base_link到laser_link
laser_transform = TransformStamped()
laser_transform.header.stamp = rospy.Time.now()
laser_transform.header.frame_id = "base_link"
laser_transform.child_frame_id = "laser_link"
laser_transform.transform.translation.x = 0.2
laser_transform.transform.translation.y = 0.0
laser_transform.transform.translation.z = 0.1
q = tf_trans.quaternion_from_euler(0, 0, 0)
laser_transform.transform.rotation.x = q[0]
laser_transform.transform.rotation.y = q[1]
laser_transform.transform.rotation.z = q[2]
laser_transform.transform.rotation.w = q[3]
static_transforms.append(laser_transform)
# 2. base_link到camera_link
camera_transform = TransformStamped()
camera_transform.header.stamp = rospy.Time.now()
camera_transform.header.frame_id = "base_link"
camera_transform.child_frame_id = "camera_link"
camera_transform.transform.translation.x = 0.15
camera_transform.transform.translation.y = 0.0
camera_transform.transform.translation.z = 0.3
# 相机向下倾斜15度
q = tf_trans.quaternion_from_euler(0, -0.2618, 0)
camera_transform.transform.rotation.x = q[0]
camera_transform.transform.rotation.y = q[1]
camera_transform.transform.rotation.z = q[2]
camera_transform.transform.rotation.w = q[3]
static_transforms.append(camera_transform)
# 3. base_link到多个传感器
sensor_configs = [
{'name': 'imu_link', 'x': 0.0, 'y': 0.0, 'z': 0.05, 'roll': 0, 'pitch': 0, 'yaw': 0},
{'name': 'gps_link', 'x': -0.1, 'y': 0.0, 'z': 0.2, 'roll': 0, 'pitch': 0, 'yaw': 0},
{'name': 'ultrasonic_front', 'x': 0.25, 'y': 0.0, 'z': 0.05, 'roll': 0, 'pitch': 0, 'yaw': 0},
{'name': 'ultrasonic_back', 'x': -0.25, 'y': 0.0, 'z': 0.05, 'roll': 0, 'pitch': 0, 'yaw': 3.14159},
]
for config in sensor_configs:
transform = self.create_transform(
"base_link",
config['name'],
config['x'], config['y'], config['z'],
config['roll'], config['pitch'], config['yaw']
)
static_transforms.append(transform)
# 发布所有静态变换
self.static_broadcaster.sendTransform(static_transforms)
rospy.loginfo(f"Published {len(static_transforms)} static transforms")
def create_transform(self, parent_frame, child_frame, x, y, z, roll, pitch, yaw):
"""创建变换消息"""
transform = TransformStamped()
transform.header.stamp = rospy.Time.now()
transform.header.frame_id = parent_frame
transform.child_frame_id = child_frame
transform.transform.translation.x = x
transform.transform.translation.y = y
transform.transform.translation.z = z
q = tf_trans.quaternion_from_euler(roll, pitch, yaw)
transform.transform.rotation.x = q[0]
transform.transform.rotation.y = q[1]
transform.transform.rotation.z = q[2]
transform.transform.rotation.w = q[3]
return transform
def publish_from_urdf(self):
"""从URDF参数发布静态变换"""
# 这通常由robot_state_publisher完成
# 但我们可以手动解析URDF并发布
if rospy.has_param('robot_description'):
robot_description = rospy.get_param('robot_description')
# 解析URDF并提取固定关节的变换
# 这里简化处理
rospy.loginfo("Would parse URDF and publish fixed joints")
def main():
publisher = StaticTransformPublisher()
rospy.spin()
if __name__ == '__main__':
main()3.3 Launch文件配置静态变换
xml
<!-- launch/static_transforms.launch -->
<launch>
<!-- 方法1：使用static_transform_publisher节点 -->
<node pkg="tf2_ros" type="static_transform_publisher"
name="laser_broadcaster"
args="0.2 0 0.1 0 0 0 base_link laser_link" />
<node pkg="tf2_ros" type="static_transform_publisher"
name="camera_broadcaster"
args="0.15 0 0.3 0 -0.2618 0 base_link camera_link" />
<!-- 方法2：使用tf包的static_transform_publisher（旧版本） -->
<node pkg="tf" type="static_transform_publisher"
name="imu_broadcaster"
args="0 0 0.05 0 0 0 base_link imu_link 100" />
<!-- 方法3：从参数加载 -->
<rosparam>
static_transforms:
- frame_id: base_link
child_frame_id: gps_link
x: -0.1
y: 0.0
z: 0.2
roll: 0.0
pitch: 0.0
yaw: 0.0
</rosparam>
<node name="param_static_publisher" pkg="my_tf_pkg" type="param_static_publisher.py" />
<!-- 方法4：从URDF自动生成 -->
<param name="robot_description" command="$(find xacro)/xacro $(find my_robot_description)/urdf/robot.urdf.xacro" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
<param name="publish_frequency" value="50" />
</node>
</launch>4. 动态坐标变换
4.1 动态变换广播器
cpp
// dynamic_transform_broadcaster.cpp
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/JointState.h>
class DynamicTransformBroadcaster {
public:
DynamicTransformBroadcaster() : nh_("~") {
// 初始化
tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>();
// 订阅里程计
odom_sub_ = nh_.subscribe("/odom", 10,
&DynamicTransformBroadcaster::odomCallback, this);
// 订阅关节状态
joint_sub_ = nh_.subscribe("/joint_states", 10,
&DynamicTransformBroadcaster::jointCallback, this);
// 定时发布变换
timer_ = nh_.createTimer(ros::Duration(0.02), // 50Hz
&DynamicTransformBroadcaster::timerCallback, this);
ROS_INFO("Dynamic transform broadcaster initialized");
}
private:
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
// 发布odom到base_link的变换
geometry_msgs::TransformStamped odom_transform;
odom_transform.header.stamp = msg->header.stamp;
odom_transform.header.frame_id = "odom";
odom_transform.child_frame_id = "base_link";
// 从里程计消息复制位置
odom_transform.transform.translation.x = msg->pose.pose.position.x;
odom_transform.transform.translation.y = msg->pose.pose.position.y;
odom_transform.transform.translation.z = msg->pose.pose.position.z;
// 从里程计消息复制姿态
odom_transform.transform.rotation = msg->pose.pose.orientation;
// 发送变换
tf_broadcaster_->sendTransform(odom_transform);
}
void jointCallback(const sensor_msgs::JointState::ConstPtr& msg) {
// 发布关节变换（例如机械臂）
for (size_t i = 0; i < msg->name.size(); ++i) {
if (msg->name[i] == "arm_joint_1") {
publishJointTransform("arm_base_link", "arm_link_1",
0, 0, 0.1, // 平移
msg->position[i], 0, 0); // 旋转（绕Z轴）
}
else if (msg->name[i] == "arm_joint_2") {
publishJointTransform("arm_link_1", "arm_link_2",
0.2, 0, 0, // 平移
0, msg->position[i], 0); // 旋转（绕Y轴）
}
// ... 更多关节
}
}
void timerCallback(const ros::TimerEvent& event) {
// 发布时变的变换（例如旋转的传感器）
ros::Time now = ros::Time::now();
// 旋转的激光雷达
double angle = 2 * M_PI * fmod(now.toSec(), 10.0) / 10.0; // 10秒一圈
geometry_msgs::TransformStamped rotating_transform;
rotating_transform.header.stamp = now;
rotating_transform.header.frame_id = "base_link";
rotating_transform.child_frame_id = "rotating_laser";
rotating_transform.transform.translation.x = 0.0;
rotating_transform.transform.translation.y = 0.0;
rotating_transform.transform.translation.z = 0.2;
tf2::Quaternion q;
q.setRPY(0, 0, angle);
rotating_transform.transform.rotation.x = q.x();
rotating_transform.transform.rotation.y = q.y();
rotating_transform.transform.rotation.z = q.z();
rotating_transform.transform.rotation.w = q.w();
tf_broadcaster_->sendTransform(rotating_transform);
}
void publishJointTransform(const std::string& parent_frame,
const std::string& child_frame,
double x, double y, double z,
double roll, double pitch, double yaw) {
geometry_msgs::TransformStamped transform;
transform.header.stamp = ros::Time::now();
transform.header.frame_id = parent_frame;
transform.child_frame_id = child_frame;
transform.transform.translation.x = x;
transform.transform.translation.y = y;
transform.transform.translation.z = z;
tf2::Quaternion q;
q.setRPY(roll, pitch, yaw);
transform.transform.rotation.x = q.x();
transform.transform.rotation.y = q.y();
transform.transform.rotation.z = q.z();
transform.transform.rotation.w = q.w();
tf_broadcaster_->sendTransform(transform);
}
ros::NodeHandle nh_;
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
ros::Subscriber odom_sub_;
ros::Subscriber joint_sub_;
ros::Timer timer_;
};
int main(int argc, char** argv) {
ros::init(argc, argv, "dynamic_transform_broadcaster");
DynamicTransformBroadcaster broadcaster;
ros::spin();
return 0;
}4.2 Python动态变换
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped
from nav_msgs.msg import Odometry
from sensor_msgs.msg import JointState
import math
class DynamicTransformBroadcaster:
"""动态坐标变换广播器"""
def __init__(self):
rospy.init_node('dynamic_transform_broadcaster')
# TF广播器
self.tf_broadcaster = tf2_ros.TransformBroadcaster()
# 订阅者
self.odom_sub = rospy.Subscriber(
'/odom', Odometry, self.odom_callback)
self.joint_sub = rospy.Subscriber(
'/joint_states', JointState, self.joint_callback)
# 定时器
self.timer = rospy.Timer(rospy.Duration(0.02), self.timer_callback)
# 状态
self.robot_x = 0.0
self.robot_y = 0.0
self.robot_theta = 0.0
rospy.loginfo("Dynamic transform broadcaster started")
def odom_callback(self, msg):
"""里程计回调"""
# 发布odom到base_link的变换
transform = TransformStamped()
transform.header.stamp = msg.header.stamp
transform.header.frame_id = "odom"
transform.child_frame_id = "base_link"
# 位置
transform.transform.translation.x = msg.pose.pose.position.x
transform.transform.translation.y = msg.pose.pose.position.y
transform.transform.translation.z = msg.pose.pose.position.z
# 姿态
transform.transform.rotation = msg.pose.pose.orientation
# 广播变换
self.tf_broadcaster.sendTransform(transform)
# 更新机器人位置（用于其他计算）
self.robot_x = msg.pose.pose.position.x
self.robot_y = msg.pose.pose.position.y
quaternion = (
msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w
)
euler = tf.transformations.euler_from_quaternion(quaternion)
self.robot_theta = euler[2]
def joint_callback(self, msg):
"""关节状态回调"""
# 发布关节变换
for i, joint_name in enumerate(msg.name):
if joint_name == "arm_joint_1":
self.publish_joint_transform(
"arm_base_link", "arm_link_1",
0, 0, 0.1, # 平移
msg.position[i], 0, 0 # 旋转
)
elif joint_name == "arm_joint_2":
self.publish_joint_transform(
"arm_link_1", "arm_link_2",
0.2, 0, 0, # 平移
0, msg.position[i], 0 # 旋转
)
elif joint_name == "gripper_joint":
# 夹爪（平移关节）
self.publish_joint_transform(
"arm_link_2", "gripper_link",
msg.position[i], 0, 0, # 平移
0, 0, 0 # 无旋转
)
def timer_callback(self, event):
"""定时器回调"""
current_time = rospy.Time.now()
# 1. 发布旋转的传感器
self.publish_rotating_sensor(current_time)
# 2. 发布摆动的天线
self.publish_swinging_antenna(current_time)
# 3. 发布模拟的轮子旋转
self.publish_wheel_rotation(current_time)
def publish_rotating_sensor(self, timestamp):
"""发布旋转的传感器"""
# 10秒转一圈
period = 10.0
angle = 2 * math.pi * (timestamp.to_sec() % period) / period
transform = TransformStamped()
transform.header.stamp = timestamp
transform.header.frame_id = "base_link"
transform.child_frame_id = "rotating_sensor"
transform.transform.translation.x = 0.0
transform.transform.translation.y = 0.0
transform.transform.translation.z = 0.3
q = tf.transformations.quaternion_from_euler(0, 0, angle)
transform.transform.rotation.x = q[0]
transform.transform.rotation.y = q[1]
transform.transform.rotation.z = q[2]
transform.transform.rotation.w = q[3]
self.tf_broadcaster.sendTransform(transform)
def publish_swinging_antenna(self, timestamp):
"""发布摆动的天线"""
# 正弦摆动
amplitude = 0.5 # 弧度
frequency = 0.5 # Hz
angle = amplitude * math.sin(2 * math.pi * frequency * timestamp.to_sec())
transform = TransformStamped()
transform.header.stamp = timestamp
transform.header.frame_id = "base_link"
transform.child_frame_id = "antenna"
transform.transform.translation.x = -0.1
transform.transform.translation.y = 0.0
transform.transform.translation.z = 0.4
q = tf.transformations.quaternion_from_euler(angle, 0, 0)
transform.transform.rotation.x = q[0]
transform.transform.rotation.y = q[1]
transform.transform.rotation.z = q[2]
transform.transform.rotation.w = q[3]
self.tf_broadcaster.sendTransform(transform)
def publish_wheel_rotation(self, timestamp):
"""发布轮子旋转"""
# 基于机器人速度估算轮子转速
wheel_radius = 0.05 # 轮子半径
linear_velocity = math.sqrt(self.robot_x**2 + self.robot_y**2) / timestamp.to_sec()
if timestamp.to_sec() > 0:
wheel_angle = linear_velocity * timestamp.to_sec() / wheel_radius
else:
wheel_angle = 0
# 左轮
transform_left = TransformStamped()
transform_left.header.stamp = timestamp
transform_left.header.frame_id = "base_link"
transform_left.child_frame_id = "wheel_left"
transform_left.transform.translation.x = 0.0
transform_left.transform.translation.y = 0.15
transform_left.transform.translation.z = -0.05
q = tf.transformations.quaternion_from_euler(0, wheel_angle, 0)
transform_left.transform.rotation.x = q[0]
transform_left.transform.rotation.y = q[1]
transform_left.transform.rotation.z = q[2]
transform_left.transform.rotation.w = q[3]
# 右轮
transform_right = TransformStamped()
transform_right.header.stamp = timestamp
transform_right.header.frame_id = "base_link"
transform_right.child_frame_id = "wheel_right"
transform_right.transform.translation.x = 0.0
transform_right.transform.translation.y = -0.15
transform_right.transform.translation.z = -0.05
transform_right.transform.rotation = transform_left.transform.rotation
self.tf_broadcaster.sendTransform([transform_left, transform_right])
def publish_joint_transform(self, parent_frame, child_frame,
x, y, z, roll, pitch, yaw):
"""发布关节变换"""
transform = TransformStamped()
transform.header.stamp = rospy.Time.now()
transform.header.frame_id = parent_frame
transform.child_frame_id = child_frame
transform.transform.translation.x = x
transform.transform.translation.y = y
transform.transform.translation.z = z
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
transform.transform.rotation.x = q[0]
transform.transform.rotation.y = q[1]
transform.transform.rotation.z = q[2]
transform.transform.rotation.w = q[3]
self.tf_broadcaster.sendTransform(transform)
def main():
broadcaster = DynamicTransformBroadcaster()
rospy.spin()
if __name__ == '__main__':
main()5. TF树管理
5.1 TF树监听器
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import tf2_ros
import tf2_geometry_msgs
from geometry_msgs.msg import PointStamped, PoseStamped
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
class TFListener:
"""TF监听器和查询"""
def __init__(self):
rospy.init_node('tf_listener')
# 创建TF缓冲区和监听器
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# 等待TF树建立
rospy.sleep(2.0)
# 演示各种TF查询
self.demonstrate_tf_queries()
# 设置定时器持续查询
self.timer = rospy.Timer(rospy.Duration(1.0), self.timer_callback)
def demonstrate_tf_queries(self):
"""演示TF查询"""
try:
# 1. 查询最新的变换
self.query_latest_transform()
# 2. 查询特定时间的变换
self.query_transform_at_time()
# 3. 变换点
self.transform_point()
# 4. 变换位姿
self.transform_pose()
# 5. 查询所有坐标系
self.list_all_frames()
# 6. 检查变换是否存在
self.check_transform_existence()
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
rospy.logerr(f"TF Error: {e}")
def query_latest_transform(self):
"""查询最新的变换"""
try:
# 获取base_link到laser_link的最新变换
transform = self.tf_buffer.lookup_transform(
'base_link', 'laser_link', rospy.Time())
rospy.loginfo("Latest transform from base_link to laser_link:")
rospy.loginfo(f" Translation: ({transform.transform.translation.x:.3f}, "
f"{transform.transform.translation.y:.3f}, "
f"{transform.transform.translation.z:.3f})")
# 提取欧拉角
import tf.transformations as tf_trans
quaternion = (
transform.transform.rotation.x,
transform.transform.rotation.y,
transform.transform.rotation.z,
transform.transform.rotation.w
)
euler = tf_trans.euler_from_quaternion(quaternion)
rospy.loginfo(f" Rotation (RPY): ({euler[0]:.3f}, {euler[1]:.3f}, {euler[2]:.3f})")
except tf2_ros.TransformException as e:
rospy.logwarn(f"Could not get transform: {e}")
def query_transform_at_time(self):
"""查询特定时间的变换"""
try:
# 查询1秒前的变换
past_time = rospy.Time.now() - rospy.Duration(1.0)
if self.tf_buffer.can_transform('odom', 'base_link', past_time):
transform = self.tf_buffer.lookup_transform(
'odom', 'base_link', past_time)
rospy.loginfo(f"Transform at {past_time.to_sec():.2f}:")
rospy.loginfo(f" Position: ({transform.transform.translation.x:.3f}, "
f"{transform.transform.translation.y:.3f})")
except tf2_ros.TransformException as e:
rospy.logwarn(f"Could not get past transform: {e}")
def transform_point(self):
"""变换点"""
# 创建一个在laser_link坐标系中的点
point_laser = PointStamped()
point_laser.header.frame_id = "laser_link"
point_laser.header.stamp = rospy.Time()
point_laser.point.x = 1.0 # 激光雷达前方1米
point_laser.point.y = 0.0
point_laser.point.z = 0.0
try:
# 变换到base_link坐标系
point_base = self.tf_buffer.transform(point_laser, "base_link")
rospy.loginfo("Point transformation:")
rospy.loginfo(f" In laser_link: ({point_laser.point.x:.3f}, "
f"{point_laser.point.y:.3f}, {point_laser.point.z:.3f})")
rospy.loginfo(f" In base_link: ({point_base.point.x:.3f}, "
f"{point_base.point.y:.3f}, {point_base.point.z:.3f})")
except tf2_ros.TransformException as e:
rospy.logwarn(f"Could not transform point: {e}")
def transform_pose(self):
"""变换位姿"""
# 创建一个位姿
pose_camera = PoseStamped()
pose_camera.header.frame_id = "camera_link"
pose_camera.header.stamp = rospy.Time()
# 相机坐标系中检测到的物体
pose_camera.pose.position.x = 2.0
pose_camera.pose.position.y = 0.5
pose_camera.pose.position.z = 0.0
pose_camera.pose.orientation.w = 1.0
try:
# 变换到map坐标系
pose_map = self.tf_buffer.transform(pose_camera, "map",
timeout=rospy.Duration(1.0))
rospy.loginfo("Object pose in map frame:")
rospy.loginfo(f" Position: ({pose_map.pose.position.x:.3f}, "
f"{pose_map.pose.position.y:.3f}, "
f"{pose_map.pose.position.z:.3f})")
except tf2_ros.TransformException as e:
rospy.logwarn(f"Could not transform pose: {e}")
def list_all_frames(self):
"""列出所有坐标系"""
frames = self.tf_buffer.all_frames_as_yaml()
rospy.loginfo("All TF frames:")
# 解析YAML输出
lines = frames.split('\n')
frame_names = [line.split(':')[0].strip()
for line in lines if ':' in line]
for frame in frame_names[:10]: # 只显示前10个
rospy.loginfo(f" - {frame}")
def check_transform_existence(self):
"""检查变换是否存在"""
frame_pairs = [
('map', 'odom'),
('odom', 'base_link'),
('base_link', 'laser_link'),
('base_link', 'non_existent_frame')
]
for source, target in frame_pairs:
if self.tf_buffer.can_transform(target, source, rospy.Time()):
rospy.loginfo(f"Transform {source} -> {target}: EXISTS")
else:
rospy.loginfo(f"Transform {source} -> {target}: NOT FOUND")
def timer_callback(self, event):
"""定时查询回调"""
try:
# 持续跟踪机器人位置
transform = self.tf_buffer.lookup_transform(
'map', 'base_link', rospy.Time())
x = transform.transform.translation.x
y = transform.transform.translation.y
rospy.loginfo(f"Robot position in map: ({x:.2f}, {y:.2f})")
except tf2_ros.TransformException:
pass # 忽略错误
def transform_point_cloud(self, cloud_msg, target_frame):
"""变换点云"""
try:
# 获取变换
transform = self.tf_buffer.lookup_transform(
target_frame,
cloud_msg.header.frame_id,
cloud_msg.header.stamp,
rospy.Duration(1.0))
# 变换点云（需要tf2_sensor_msgs）
# transformed_cloud = tf2_sensor_msgs.do_transform_cloud(cloud_msg, transform)
rospy.loginfo(f"Transformed point cloud from {cloud_msg.header.frame_id} to {target_frame}")
except tf2_ros.TransformException as e:
rospy.logwarn(f"Could not transform point cloud: {e}")
def main():
listener = TFListener()
rospy.spin()
if __name__ == '__main__':
main()6. TF监听和查询
6.1 高级TF查询
cpp
// tf_advanced_listener.cpp
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
class AdvancedTFListener {
public:
AdvancedTFListener() : tfListener_(tfBuffer_) {
// 等待TF缓冲区填充
ros::Duration(2.0).sleep();
// 创建定时器
timer_ = nh_.createTimer(ros::Duration(0.1),
&AdvancedTFListener::timerCallback, this);
ROS_INFO("Advanced TF listener initialized");
}
private:
void timerCallback(const ros::TimerEvent& event) {
// 1. 获取变换矩阵
getTransformMatrix();
// 2. 计算相对速度
calculateRelativeVelocity();
// 3. 跟踪移动物体
trackMovingObject();
// 4. 坐标系链查询
queryTransformChain();
}
void getTransformMatrix() {
try {
// 获取变换
geometry_msgs::TransformStamped transform =
tfBuffer_.lookupTransform("map", "base_link", ros::Time(0));
// 转换为矩阵形式
tf2::Transform tf_transform;
tf2::fromMsg(transform.transform, tf_transform);
// 获取矩阵元素
tf2::Matrix3x3 rotation = tf_transform.getBasis();
tf2::Vector3 translation = tf_transform.getOrigin();
ROS_INFO_THROTTLE(5, "Transform matrix from map to base_link:");
for (int i = 0; i < 3; ++i) {
ROS_INFO_THROTTLE(5, " [%.3f, %.3f, %.3f, %.3f]",
rotation[i][0], rotation[i][1], rotation[i][2],
i == 0 ? translation.x() : (i == 1 ? translation.y() : translation.z()));
}
}
catch (tf2::TransformException& ex) {
ROS_WARN_THROTTLE(1, "Transform error: %s", ex.what());
}
}
void calculateRelativeVelocity() {
try {
// 获取两个时间点的变换
ros::Time now = ros::Time::now();
ros::Time past = now - ros::Duration(0.1);
// 确保变换可用
if (!tfBuffer_.canTransform("odom", "base_link", now) ||
!tfBuffer_.canTransform("odom", "base_link", past)) {
return;
}
geometry_msgs::TransformStamped transform_now =
tfBuffer_.lookupTransform("odom", "base_link", now);
geometry_msgs::TransformStamped transform_past =
tfBuffer_.lookupTransform("odom", "base_link", past);
// 计算位移
double dx = transform_now.transform.translation.x -
transform_past.transform.translation.x;
double dy = transform_now.transform.translation.y -
transform_past.transform.translation.y;
// 计算速度
double dt = (now - past).toSec();
double vx = dx / dt;
double vy = dy / dt;
ROS_INFO_THROTTLE(1, "Robot velocity: vx=%.2f, vy=%.2f m/s", vx, vy);
}
catch (tf2::TransformException& ex) {
// 忽略错误
}
}
void trackMovingObject() {
// 模拟跟踪移动物体
static double object_x = 5.0;
static double object_y = 0.0;
// 物体在世界坐标系中移动
object_x += 0.01;
object_y = 2.0 * sin(object_x * 0.5);
// 创建物体位置（世界坐标系）
geometry_msgs::PointStamped object_world;
object_world.header.frame_id = "map";
object_world.header.stamp = ros::Time();
object_world.point.x = object_x;
object_world.point.y = object_y;
object_world.point.z = 0.0;
try {
// 变换到机器人坐标系
geometry_msgs::PointStamped object_robot =
tfBuffer_.transform(object_world, "base_link");
// 计算距离和角度
double distance = sqrt(pow(object_robot.point.x, 2) +
pow(object_robot.point.y, 2));
double angle = atan2(object_robot.point.y, object_robot.point.x);
ROS_INFO_THROTTLE(1, "Object relative position: distance=%.2f, angle=%.2f°",
distance, angle * 180.0 / M_PI);
}
catch (tf2::TransformException& ex) {
ROS_WARN("Could not track object: %s", ex.what());
}
}
void queryTransformChain() {
// 查询变换链
std::vector<std::string> chain = {"map", "odom", "base_link", "laser_link"};
for (size_t i = 0; i < chain.size() - 1; ++i) {
try {
geometry_msgs::TransformStamped transform =
tfBuffer_.lookupTransform(chain[i], chain[i+1], ros::Time(0));
ROS_INFO_THROTTLE(10, "Transform %s -> %s exists",
chain[i].c_str(), chain[i+1].c_str());
}
catch (tf2::TransformException& ex) {
ROS_WARN("Transform %s -> %s missing: %s",
chain[i].c_str(), chain[i+1].c_str(), ex.what());
}
}
}
ros::NodeHandle nh_;
tf2_ros::Buffer tfBuffer_;
tf2_ros::TransformListener tfListener_;
ros::Timer timer_;
};
int main(int argc, char** argv) {
ros::init(argc, argv, "tf_advanced_listener");
AdvancedTFListener listener;
ros::spin();
return 0;
}7. TF工具使用
7.1 命令行工具
bash
# 1. 查看TF树
rosrun tf2_tools view_frames.py
# 生成frames.pdf文件，显示完整的TF树
# 2. 打印TF树
rosrun tf tf_echo source_frame target_frame
# 实时显示两个坐标系之间的变换
# 3. 监控TF
rosrun tf tf_monitor
# 显示所有TF广播器的统计信息
# 4. 静态变换发布
rosrun tf2_ros static_transform_publisher x y z yaw pitch roll frame_id child_frame_id
# 5. RViz中查看TF
# 启动RViz并添加TF显示插件
# 6. rqt中的TF树
rosrun rqt_tf_tree rqt_tf_tree
# 图形化显示TF树结构
# 7. 保存和回放TF
rosbag record -O tf_data.bag /tf /tf_static
rosbag play tf_data.bag
# 8. 查询特定时刻的变换
rosrun tf tf_echo -t 1234567890.123 map base_link
# 9. 检查TF延迟
rostopic echo /tf_static
rostopic hz /tf7.2 Python TF工具
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import tf2_ros
import tf2_py as tf2
from tf2_msgs.msg import TFMessage
import yaml
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import networkx as nx
class TFAnalyzer:
"""TF分析工具"""
def __init__(self):
rospy.init_node('tf_analyzer')
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# TF统计
self.tf_stats = {}
self.tf_sub = rospy.Subscriber('/tf', TFMessage, self.tf_callback)
rospy.sleep(2.0)
def tf_callback(self, msg):
"""TF消息回调"""
for transform in msg.transforms:
key = f"{transform.header.frame_id}->{transform.child_frame_id}"
if key not in self.tf_stats:
self.tf_stats[key] = {
'count': 0,
'last_time': None,
'frequency': 0
}
stats = self.tf_stats[key]
stats['count'] += 1
if stats['last_time']:
dt = (transform.header.stamp - stats['last_time']).to_sec()
if dt > 0:
stats['frequency'] = 1.0 / dt
stats['last_time'] = transform.header.stamp
def analyze_tf_tree(self):
"""分析TF树结构"""
# 获取所有坐标系
yaml_str = self.tf_buffer.all_frames_as_yaml()
frames_data = yaml.safe_load(yaml_str)
print("=== TF Tree Analysis ===")
print(f"Total frames: {len(frames_data)}")
# 构建图
G = nx.DiGraph()
for frame_name, frame_data in frames_data.items():
if 'parent' in frame_data:
parent = frame_data['parent']
G.add_edge(parent, frame_name)
# 分析图结构
print(f"Tree depth: {nx.dag_longest_path_length(G)}")
print(f"Leaf nodes: {[n for n in G.nodes() if G.out_degree(n) == 0]}")
# 查找根节点
roots = [n for n in G.nodes() if G.in_degree(n) == 0]
print(f"Root frames: {roots}")
return G
def print_tf_statistics(self):
"""打印TF统计信息"""
print("\n=== TF Statistics ===")
print(f"{'Transform':<30} {'Count':<10} {'Frequency':<10}")
print("-" * 50)
for key, stats in sorted(self.tf_stats.items()):
print(f"{key:<30} {stats['count']:<10} {stats['frequency']:<10.1f}")
def check_tf_delays(self):
"""检查TF延迟"""
print("\n=== TF Delays ===")
current_time = rospy.Time.now()
for key, stats in self.tf_stats.items():
if stats['last_time']:
delay = (current_time - stats['last_time']).to_sec()
if delay > 0.5: # 超过0.5秒认为是延迟
print(f"{key}: {delay:.2f} seconds delay")
def visualize_tf_tree(self, graph):
"""可视化TF树"""
plt.figure(figsize=(12, 8))
# 使用层次布局
pos = nx.spring_layout(graph)
# 绘制节点和边
nx.draw(graph, pos, with_labels=True,
node_color='lightblue',
node_size=2000,
font_size=8,
font_weight='bold',
arrows=True,
edge_color='gray')
plt.title("TF Tree Structure")
plt.axis('off')
plt.tight_layout()
plt.show()
def monitor_transform_rate(self, source_frame, target_frame, duration=10):
"""监控变换频率"""
print(f"\nMonitoring {source_frame} -> {target_frame} for {duration} seconds...")
start_time = rospy.Time.now()
count = 0
errors = 0
while (rospy.Time.now() - start_time).to_sec() < duration:
try:
transform = self.tf_buffer.lookup_transform(
target_frame, source_frame, rospy.Time())
count += 1
except tf2_ros.TransformException:
errors += 1
rospy.sleep(0.01)
elapsed = (rospy.Time.now() - start_time).to_sec()
rate = count / elapsed
print(f"Results:")
print(f" Successful lookups: {count}")
print(f" Failed lookups: {errors}")
print(f" Average rate: {rate:.1f} Hz")
def find_transform_path(self, source_frame, target_frame):
"""查找变换路径"""
print(f"\nFinding path from {source_frame} to {target_frame}...")
# 构建TF图
yaml_str = self.tf_buffer.all_frames_as_yaml()
frames_data = yaml.safe_load(yaml_str)
G = nx.DiGraph()
for frame_name, frame_data in frames_data.items():
if 'parent' in frame_data:
parent = frame_data['parent']
G.add_edge(parent, frame_name)
G.add_edge(frame_name, parent) # 双向
# 查找路径
try:
path = nx.shortest_path(G, source_frame, target_frame)
print(f"Path found: {' -> '.join(path)}")
return path
except nx.NetworkXNoPath:
print(f"No path found between {source_frame} and {target_frame}")
return None
def main():
analyzer = TFAnalyzer()
# 等待TF数据
rospy.sleep(3.0)
# 执行分析
graph = analyzer.analyze_tf_tree()
analyzer.print_tf_statistics()
analyzer.check_tf_delays()
# 监控特定变换
if analyzer.tf_buffer.can_transform("map", "base_link", rospy.Time()):
analyzer.monitor_transform_rate("map", "base_link", duration=5)
# 查找路径
analyzer.find_transform_path("map", "laser_link")
# 可视化
# analyzer.visualize_tf_tree(graph)
rospy.spin()
if __name__ == '__main__':
main()8. TF2高级特性
8.1 TF2特性示例
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import tf2_ros
import tf2_py as tf2
from geometry_msgs.msg import TransformStamped, PoseStamped, PointStamped
class TF2AdvancedFeatures:
"""TF2高级特性"""
def __init__(self):
rospy.init_node('tf2_advanced_features')
# 创建多个缓冲区（TF2特性）
self.main_buffer = tf2_ros.Buffer()
self.main_listener = tf2_ros.TransformListener(self.main_buffer)
# 专用缓冲区（用于不同用途）
self.planning_buffer = tf2_ros.Buffer(rospy.Duration(30.0)) # 30秒历史
self.planning_listener = tf2_ros.TransformListener(self.planning_buffer)
rospy.sleep(2.0)
# 演示高级特性
self.demonstrate_advanced_features()
def demonstrate_advanced_features(self):
"""演示TF2高级特性"""
# 1. 时间旅行查询
self.time_travel_queries()
# 2. 变换缓存管理
self.buffer_management()
# 3. 异步变换查询
self.async_transform_queries()
# 4. 自定义插值
self.custom_interpolation()
# 5. 坐标系权威管理
self.frame_authority()
def time_travel_queries(self):
"""时间旅行查询"""
print("=== Time Travel Queries ===")
try:
# 查询过去的变换
past_time = rospy.Time.now() - rospy.Duration(5.0)
if self.planning_buffer.can_transform("map", "base_link", past_time):
past_transform = self.planning_buffer.lookup_transform(
"map", "base_link", past_time)
print(f"Position 5 seconds ago: "
f"({past_transform.transform.translation.x:.2f}, "
f"{past_transform.transform.translation.y:.2f})")
# 查询最早可用的变换时间
try:
earliest_time = self.planning_buffer.get_latest_common_time(
"map", "base_link")
print(f"Earliest common time: {earliest_time.to_sec():.2f}")
except:
pass
except tf2_ros.TransformException as e:
print(f"Time travel query failed: {e}")
def buffer_management(self):
"""缓冲区管理"""
print("\n=== Buffer Management ===")
# 清除特定变换
# self.main_buffer.clear() # 清除所有
# 设置缓冲区大小
custom_buffer = tf2_ros.Buffer(cache_time=rospy.Duration(60.0))
# 检查缓冲区状态
if self.main_buffer.can_transform("map", "base_link", rospy.Time()):
print("Transform available in buffer")
# 等待变换可用
try:
self.main_buffer.lookup_transform(
"map", "base_link",
rospy.Time(),
timeout=rospy.Duration(1.0))
print("Transform lookup successful")
except:
print("Transform lookup timeout")
def async_transform_queries(self):
"""异步变换查询"""
print("\n=== Async Transform Queries ===")
# 注册回调
def transform_callback(transform):
print(f"Async transform received: "
f"({transform.transform.translation.x:.2f}, "
f"{transform.transform.translation.y:.2f})")
# 异步查询（注意：这是概念演示，实际API可能不同）
# future = self.main_buffer.lookup_transform_async(
# "map", "base_link", rospy.Time())
# future.add_done_callback(transform_callback)
def custom_interpolation(self):
"""自定义插值"""
print("\n=== Custom Interpolation ===")
try:
# 获取两个时间点的变换
time1 = rospy.Time.now() - rospy.Duration(1.0)
time2 = rospy.Time.now()
if (self.main_buffer.can_transform("odom", "base_link", time1) and
self.main_buffer.can_transform("odom", "base_link", time2)):
transform1 = self.main_buffer.lookup_transform(
"odom", "base_link", time1)
transform2 = self.main_buffer.lookup_transform(
"odom", "base_link", time2)
# 线性插值位置
alpha = 0.5 # 插值系数
interp_x = (1-alpha) * transform1.transform.translation.x + \
alpha * transform2.transform.translation.x
interp_y = (1-alpha) * transform1.transform.translation.y + \
alpha * transform2.transform.translation.y
print(f"Interpolated position: ({interp_x:.2f}, {interp_y:.2f})")
except tf2_ros.TransformException as e:
print(f"Interpolation failed: {e}")
def frame_authority(self):
"""坐标系权威管理"""
print("\n=== Frame Authority ===")
# 检查变换的权威性
try:
transform = self.main_buffer.lookup_transform(
"map", "base_link", rospy.Time())
# 获取发布者信息
authority = transform.header.frame_id
child = transform.child_frame_id
print(f"Transform authority: {authority} -> {child}")
# 时间戳信息
stamp = transform.header.stamp
age = (rospy.Time.now() - stamp).to_sec()
print(f"Transform age: {age:.3f} seconds")
except tf2_ros.TransformException:
pass
class TF2MessageFilter:
"""TF2消息过滤器"""
def __init__(self):
import message_filters
from sensor_msgs.msg import PointCloud2
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# 创建消息过滤器
self.point_cloud_sub = message_filters.Subscriber(
'/velodyne_points', PointCloud2)
# TF2消息过滤器
self.tf_filter = tf2_ros.MessageFilter(
self.point_cloud_sub,
self.tf_buffer,
'map', # 目标坐标系
10, # 队列大小
rospy.Duration(0.1)) # 允许的时间差
self.tf_filter.registerCallback(self.point_cloud_callback)
def point_cloud_callback(self, msg):
"""点云回调（已经可以变换到map坐标系）"""
rospy.loginfo(f"Received point cloud that can be transformed to map frame")
try:
# 此时保证可以进行变换
transform = self.tf_buffer.lookup_transform(
'map', msg.header.frame_id, msg.header.stamp)
# 处理点云...
rospy.loginfo("Processing point cloud in map frame")
except tf2_ros.TransformException as e:
rospy.logwarn(f"Transform error: {e}")
def main():
features = TF2AdvancedFeatures()
# filter = TF2MessageFilter()
rospy.spin()
if __name__ == '__main__':
main()9. 实战案例：机器人坐标系统
9.1 完整的机器人TF系统
python
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped
from nav_msgs.msg import Odometry
from sensor_msgs.msg import JointState, LaserScan
from visualization_msgs.msg import MarkerArray, Marker
import numpy as np
class RobotTFSystem:
"""完整的机器人TF系统"""
def __init__(self):
rospy.init_node('robot_tf_system')
# TF广播器
self.static_broadcaster = tf2_ros.StaticTransformBroadcaster()
self.dynamic_broadcaster = tf2_ros.TransformBroadcaster()
# TF监听器
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# 初始化机器人状态
self.robot_x = 0.0
self.robot_y = 0.0
self.robot_theta = 0.0
# 设置静态变换
self.setup_static_transforms()
# 订阅者
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
self.joint_sub = rospy.Subscriber('/joint_states', JointState, self.joint_callback)
self.scan_sub = rospy.Subscriber('/scan', LaserScan, self.scan_callback)
# 发布者
self.marker_pub = rospy.Publisher('/tf_markers', MarkerArray, queue_size=10)
# 定时器
self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback)
rospy.loginfo("Robot TF System initialized")
def setup_static_transforms(self):
"""设置静态坐标变换"""
static_transforms = []
# 机器人传感器配置
sensor_configs = [
# (parent, child, x, y, z, roll, pitch, yaw)
('base_link', 'laser_link', 0.2, 0.0, 0.1, 0, 0, 0),
('base_link', 'camera_link', 0.15, 0.0, 0.3, 0, -0.26, 0),
('base_link', 'imu_link', 0.0, 0.0, 0.05, 0, 0, 0),
('base_link', 'gps_link', -0.1, 0.0, 0.2, 0, 0, 0),
# 轮子
('base_link', 'wheel_left_link', 0.0, 0.15, -0.05, 0, 0, 0),
('base_link', 'wheel_right_link', 0.0, -0.15, -0.05, 0, 0, 0),
('base_link', 'caster_link', -0.15, 0.0, -0.05, 0, 0, 0),
# 机械臂基座
('base_link', 'arm_base_link', 0.1, 0.0, 0.1, 0, 0, 0),
]
for config in sensor_configs:
transform = self.create_static_transform(*config)
static_transforms.append(transform)
# 发布所有静态变换
self.static_broadcaster.sendTransform(static_transforms)
rospy.loginfo(f"Published {len(static_transforms)} static transforms")
def create_static_transform(self, parent, child, x, y, z, roll, pitch, yaw):
"""创建静态变换"""
transform = TransformStamped()
transform.header.stamp = rospy.Time.now()
transform.header.frame_id = parent
transform.child_frame_id = child
transform.transform.translation.x = x
transform.transform.translation.y = y
transform.transform.translation.z = z
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
transform.transform.rotation.x = q[0]
transform.transform.rotation.y = q[1]
transform.transform.rotation.z = q[2]
transform.transform.rotation.w = q[3]
return transform
def odom_callback(self, msg):
"""里程计回调 - 发布odom->base_link变换"""
# 发布变换
odom_trans = TransformStamped()
odom_trans.header.stamp = msg.header.stamp
odom_trans.header.frame_id = "odom"
odom_trans.child_frame_id = "base_link"
odom_trans.transform.translation.x = msg.pose.pose.position.x
odom_trans.transform.translation.y = msg.pose.pose.position.y
odom_trans.transform.translation.z = msg.pose.pose.position.z
odom_trans.transform.rotation = msg.pose.pose.orientation
self.dynamic_broadcaster.sendTransform(odom_trans)
# 更新机器人位置
self.robot_x = msg.pose.pose.position.x
self.robot_y = msg.pose.pose.position.y
q = (msg.pose.pose.orientation.x,
msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z,
msg.pose.pose.orientation.w)
euler = tf.transformations.euler_from_quaternion(q)
self.robot_theta = euler[2]
def joint_callback(self, msg):
"""关节状态回调 - 发布机械臂关节变换"""
joint_transforms = []
for i, name in enumerate(msg.name):
transform = None
if name == "arm_joint_1":
# 第一个关节（旋转）
transform = self.create_joint_transform(
"arm_base_link", "arm_link_1",
0, 0, 0.1,
0, 0, msg.position[i]
)
elif name == "arm_joint_2":
# 第二个关节（俯仰）
transform = self.create_joint_transform(
"arm_link_1", "arm_link_2",
0.2, 0, 0,
0, msg.position[i], 0
)
elif name == "arm_joint_3":
# 第三个关节
transform = self.create_joint_transform(
"arm_link_2", "arm_link_3",
0.2, 0, 0,
0, msg.position[i], 0
)
elif name == "gripper_joint":
# 夹爪（平移关节）
transform = self.create_joint_transform(
"arm_link_3", "gripper_link",
0.1 + msg.position[i], 0, 0,
0, 0, 0
)
if transform:
joint_transforms.append(transform)
if joint_transforms:
self.dynamic_broadcaster.sendTransform(joint_transforms)
def create_joint_transform(self, parent, child, x, y, z, roll, pitch, yaw):
"""创建关节变换"""
transform = TransformStamped()
transform.header.stamp = rospy.Time.now()
transform.header.frame_id = parent
transform.child_frame_id = child
transform.transform.translation.x = x
transform.transform.translation.y = y
transform.transform.translation.z = z
q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
transform.transform.rotation.x = q[0]
transform.transform.rotation.y = q[1]
transform.transform.rotation.z = q[2]
transform.transform.rotation.w = q[3]
return transform
def scan_callback(self, msg):
"""激光扫描回调 - 演示坐标变换应用"""
try:
# 将激光点变换到map坐标系
if self.tf_buffer.can_transform("map", msg.header.frame_id, msg.header.stamp):
# 获取变换
transform = self.tf_buffer.lookup_transform(
"map", msg.header.frame_id, msg.header.stamp)
# 变换一些关键点（最近点、最远点等）
min_range = min(r for r in msg.ranges if msg.range_min < r < msg.range_max)
min_index = msg.ranges.index(min_range)
min_angle = msg.angle_min + min_index * msg.angle_increment
# 激光坐标系中的点
laser_x = min_range * np.cos(min_angle)
laser_y = min_range * np.sin(min_angle)
# 这里可以应用变换...
# (简化处理，实际需要完整的变换计算)
except tf2_ros.TransformException:
pass
def timer_callback(self, event):
"""定时器回调"""
# 发布一些动态变换
self.publish_dynamic_transforms()
# 发布可视化标记
self.publish_tf_markers()
# 执行TF查询示例
self.perform_tf_queries()
def publish_dynamic_transforms(self):
"""发布动态变换"""
now = rospy.Time.now()
# 1. 模拟map->odom的变换（SLAM或定位的输出）
map_to_odom = TransformStamped()
map_to_odom.header.stamp = now
map_to_odom.header.frame_id = "map"
map_to_odom.child_frame_id = "odom"
# 简单的漂移模拟
drift_x = 0.01 * np.sin(now.to_sec() * 0.1)
drift_y = 0.01 * np.cos(now.to_sec() * 0.1)
map_to_odom.transform.translation.x = drift_x
map_to_odom.transform.translation.y = drift_y
map_to_odom.transform.translation.z = 0.0
q = tf.transformations.quaternion_from_euler(0, 0, 0)
map_to_odom.transform.rotation.x = q[0]
map_to_odom.transform.rotation.y = q[1]
map_to_odom.transform.rotation.z = q[2]
map_to_odom.transform.rotation.w = q[3]
self.dynamic_broadcaster.sendTransform(map_to_odom)
# 2. 旋转的传感器
rotating_sensor = TransformStamped()
rotating_sensor.header.stamp = now
rotating_sensor.header.frame_id = "base_link"
rotating_sensor.child_frame_id = "rotating_sensor"
angle = (now.to_sec() % 10) * 2 * np.pi / 10
rotating_sensor.transform.translation.x = 0.0
rotating_sensor.transform.translation.y = 0.0
rotating_sensor.transform.translation.z = 0.35
q = tf.transformations.quaternion_from_euler(0, 0, angle)
rotating_sensor.transform.rotation.x = q[0]
rotating_sensor.transform.rotation.y = q[1]
rotating_sensor.transform.rotation.z = q[2]
rotating_sensor.transform.rotation.w = q[3]
self.dynamic_broadcaster.sendTransform(rotating_sensor)
def publish_tf_markers(self):
"""发布TF可视化标记"""
marker_array = MarkerArray()
# 获取所有坐标系
try:
# 创建坐标系标记
frames = ['map', 'odom', 'base_link', 'laser_link', 'camera_link']
for i, frame in enumerate(frames):
if self.tf_buffer.can_transform("map", frame, rospy.Time()):
transform = self.tf_buffer.lookup_transform(
"map", frame, rospy.Time())
# 创建标记
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
marker.id = i
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.pose.position.x = transform.transform.translation.x
marker.pose.position.y = transform.transform.translation.y
marker.pose.position.z = transform.transform.translation.z + 0.1
marker.text = frame
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker_array.markers.append(marker)
except tf2_ros.TransformException:
pass
self.marker_pub.publish(marker_array)
def perform_tf_queries(self):
"""执行TF查询示例"""
try:
# 查询机器人在地图中的位置
if self.tf_buffer.can_transform("map", "base_link", rospy.Time()):
transform = self.tf_buffer.lookup_transform(
"map", "base_link", rospy.Time())
x = transform.transform.translation.x
y = transform.transform.translation.y
rospy.loginfo_throttle(5, f"Robot in map: ({x:.2f}, {y:.2f})")
except tf2_ros.TransformException:
pass
def main():
system = RobotTFSystem()
rospy.spin()
if __name__ == '__main__':
main()10. 总结与最佳实践
10.1 本文总结
通过本文的学习，你已经掌握了：
- ✅ TF系统原理：理解坐标变换的数学基础和ROS实现
- ✅ 静态变换：设置固定的坐标系关系
- ✅ 动态变换：处理时变的坐标系
- ✅ TF树管理：构建和维护坐标系树结构
- ✅ TF监听查询：获取任意坐标系间的变换
- ✅ TF工具使用：调试和可视化工具
- ✅ TF2新特性：缓冲区管理、异步查询等
- ✅ 实战应用：完整的机器人坐标系统
10.2 TF最佳实践
| 方面 | 建议 | 原因 |
|---|---|---|
| 命名规范 | 使用描述性名称，添加_link后缀 | 提高可读性 |
| 树结构 | 保持单一父节点，避免循环 | 确保计算效率 |
| 更新频率 | 静态变换用static_transform | 减少网络流量 |
| 时间戳 | 使用消息的时间戳而非now() | 保证时间一致性 |
| 坐标系选择 | 遵循REP-105标准 | 保证兼容性 |
| 错误处理 | 总是捕获TF异常 | 提高鲁棒性 |
10.3 常见问题解决
-
"未来"时间戳错误
- 问题：时间戳在未来
- 解决：检查时钟同步，使用消息时间戳
-
变换不存在
- 问题：无法查询变换
- 解决：检查TF树完整性，增加等待时间
-
TF树断裂
- 问题：坐标系未连接
- 解决：使用tf_monitor检查，补充缺失变换
-
性能问题
- 问题：TF查询缓慢
- 解决：减少查询频率，使用缓存
10.4 下一步学习
在下一篇文章中，我们将学习：
- URDF机器人建模：从零构建机器人模型
- 关节和连杆定义：机器人结构描述
- 可视化和碰撞模型：3D模型集成
- Gazebo仿真集成：物理仿真
版权声明：本文为原创文章，转载请注明出处
💡 学习建议：TF是ROS中最重要的概念之一，建议通过实践深入理解。从简单的静态变换开始，逐步学习动态变换和复杂的坐标系管理。使用RViz等可视化工具帮助理解坐标系关系。
下一篇预告：《【ROS1从入门到精通】第10篇：URDF机器人建模（从零构建机器人模型）》
敬请期待！