机器人坐标变换TF(ROS Transform)示例解释

1. TF发布

  • 静态变换 :优先通过Launch文件配置,确保启动时自动加载 。
  • 动态变换 :需在代码中循环发布(频率通常≥10Hz),避免TF断开。
  • 坐标系设计 :传感器数据绑定到自身坐标系(如base_scan)。
  • 机器人本体坐标系层级:world→map→odom→base_footprint→base_link→传感器坐标系 。

1.1 终端静态发布

用于发布固定不变的坐标系变换 (如传感器与机器人本体的固定关系),常用工具为 static_transform_publisher,语法如下:

bash 复制代码
#参数说明 :
#x y z:父坐标系到子坐标系的平移向量 (单位:米)。
#yaw pitch roll:绕Z-Y-X轴的欧拉角旋转 (单位:弧度)。
#frame_id:父坐标系名称(如base_link)。
#child_frame_id:子坐标系名称(如laser)。
#period_in_ms:发布周期(单位:毫秒,通常设为100ms)。
rosrun tf static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms


#示例:
#发布base_link到base_scan的变换(平移0.1m向前,0.2m向上,无旋转),即将base_link主题的数据(x y z yaw pitch roll),进行变换为(x+0.1 y z+0.2 yaw pitch roll)发布到base_scan主题:
# 物理意义验证,假设机器人基座(base_link)位于 (x, y, z),激光雷达(base_scan)安装在基座前方 0.1m 和上方 0.2m 处,通过这个命令,激光雷达的坐标会正确表示为 base_link 坐标加 (0.1, 0, 0.2),符合实际安装位置。
rosrun tf static_transform_publisher 0.1 0 0.2 0 0 0 base_link base_scan 100

1.2 Launch文件静态发布

xml 复制代码
<!-- 语法 含义参考命令终端-->
<node pkg="tf" type="static_transform_publisher" name="transform_name"
      args="x y z yaw pitch roll frame_id child_frame_id period_in_ms"/>

<!--# 示例-->
<node pkg="tf" type="static_transform_publisher" name="laser_tf"
      args="0.1 0 0.2 0 0 0 base_link base_scan 100"/>

1.3 动态TF发布

用于发布实时变化的坐标系变换 (如机器人本体运动),需通过代码实现,下面示例发布odom到base_footprint的动态变换(里程计坐标系到机器人2D投影坐标系)。

python 复制代码
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped

rospy.init_node('dynamic_tf_publisher')
broadcaster = tf2_ros.TransformBroadcaster()
rate = rospy.Rate(10)  # 10Hz

while not rospy.is_shutdown():
    transform = TransformStamped()
    transform.header.stamp = rospy.Time.now()
    transform.header.frame_id = "odom"
    transform.child_frame_id = "base_footprint"
    transform.transform.translation.x = 1.0
    transform.transform.rotation.w = 1.0
    broadcaster.sendTransform(transform)
    rate.sleep()

1.4 发布规则

  • 父子坐标系关系 :每个TF变换需明确父坐标系(frame_id)和子坐标系(child_frame_id),变换方向为父→子 。
  • 时间戳同步 :动态TF需使用ros::Time::now()确保时间戳同步,避免TF缓存错误。
  • 多机器人场景 :多机器人系统中需为每个机器人命名独立坐标系(如robot1/base_link),并通过world坐标系统一管理

2. 订阅监听

2.1 监听特定坐标系变换

使用 tf_echo 工具实时监听两个坐标系之间的变换:

bash 复制代码
# 参数说明 :
#frame_id:父坐标系名称(如map)。
#child_frame_id:子坐标系名称(如base_link)。
rosrun tf tf_echo frame_id child_frame_id

# 示例:监听map到base_link的变换:
# 输出内容包含平移(translation)、旋转(rotation)和时间戳等信息
rosrun tf tf_echo map base_link

2.2 查看整个TF树

bash 复制代码
#在线查看
rosrun rqt_tf_tree rqt_tf_tree
# 生成文件
rosrun tf view_frames



2.3 代码动态监听

python 复制代码
import rospy
import tf2_ros

rospy.init_node('tf_subscriber')
buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buffer)

rate = rospy.Rate(10)  # 10Hz
while not rospy.is_shutdown():
    try:
        transform = buffer.lookup_transform("odom", "base_footprint", rospy.Time(0))
        print("Current transform:", transform.transform)
    except tf2_ros.LookupException:
        continue
    rate.sleep()

2.4 订阅规则

  • 时间戳同步 :

    使用rospy.Time(0)获取最新缓存数据,避免因时间戳不匹配导致失败。

    若需精确时间点的变换(如传感器数据同步),需确保时间戳对齐 。

  • TF缓存机制 :

    TransformListener默认缓存最近5秒的TF数据,可通过参数调整缓存时长:

    buffer = tf2_ros.Buffer(ros::Duration(10.0))

  • 多机器人场景 :在多机器人系统中,需通过命名空间区分坐标系(如robot1/base_link),并通过world坐标系统一管理

相关推荐
计算机sci论文精选5 小时前
CVPR 前沿洞察 | 人机交互论文出圈,引领交互模式变革
计算机网络·机器学习·机器人·人机交互·cvpr·计算机系统·并行与分布计算
WSSWWWSSW18 小时前
认识自我的机器人:麻省理工学院基于视觉的系统让机器了解自身机体
人工智能·机器人
WSSWWWSSW21 小时前
基于模拟的流程为灵巧机器人定制训练数据
人工智能·chatgpt·机器人
菠萝炒饭pineapple-boss21 小时前
ElastAlert通过飞书机器人发送报警通知
elk·机器人·飞书
王小王-1231 天前
基于Transform、ARIMA、LSTM、Prophet的药品销量预测分析
lstm·arima·transform·prophet·药品销量预测·时序建模预测
Zhichao_971 天前
【ROS1】09-ROS通信机制——参数服务器
ros
go54631584652 天前
基于阿里云平台的文章评价模型训练与应用全流程指南
图像处理·人工智能·深度学习·阿里云·cnn·机器人·云计算
Blossom.1182 天前
基于深度学习的图像分类:使用EfficientNet实现高效分类
人工智能·python·深度学习·机器学习·分类·数据挖掘·机器人
小森( ﹡ˆoˆ﹡ )2 天前
LangChain聊天机器人教程
大数据·langchain·机器人
诸葛务农2 天前
人形机器人双足行走动力学:K-V模型其肌腱特性拟合中的应用
人工智能·算法·机器人