【无标题】

转载 已于 2026-03-23 10:58:18 修改 · 31 阅读 · 0 ·
本内容遵循CC 4.0 BY-SA版权协议
原文链接：https://blog.csdn.net/weixin_52694742/article/details/156168209
GEO检测
标签
#ros
于 2026-03-23 10:56:40 首次发布

【ROS1从入门到精通】第9篇：TF坐标变换（机器人的空间认知）

🎯 本文目标：深入理解ROS TF(Transform)系统，掌握坐标变换的原理和实现，学会管理机器人的坐标系树，能够处理复杂的空间变换问题，实现机器人的精确定位和导航。

📑 目录

  1. TF系统概述
  2. 坐标系基础理论
  3. 静态坐标变换
  4. 动态坐标变换
  5. TF树管理
  6. TF监听和查询
  7. TF工具使用
  8. TF2高级特性
  9. 实战案例：机器人坐标系统
  10. 总结与最佳实践

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的改进版本，主要区别：

特性TFTF2
API设计较旧，有些混乱更清晰、模块化
性能一般更高效
多机器人支持有限完整支持
静态变换需要节点持续发布独立的static_transform
缓冲区全局缓冲每个监听器独立缓冲
Python支持完整完整且更高效

2. 坐标系基础理论

2.1 坐标变换数学基础
#!/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:
“”“坐标变换数学基础”“”

<span class="token keyword">def</span> <span class="token function">__init__</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    self<span class="token punctuation">.</span>demonstrate_transform_math<span class="token punctuation">(</span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">demonstrate_transform_math</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""演示坐标变换数学"""</span>

    <span class="token comment"># 1. 欧拉角与四元数转换</span>
    self<span class="token punctuation">.</span>euler_quaternion_conversion<span class="token punctuation">(</span><span class="token punctuation">)</span>

    <span class="token comment"># 2. 旋转矩阵</span>
    self<span class="token punctuation">.</span>rotation_matrix_demo<span class="token punctuation">(</span><span class="token punctuation">)</span>

    <span class="token comment"># 3. 齐次变换矩阵</span>
    self<span class="token punctuation">.</span>homogeneous_transform<span class="token punctuation">(</span><span class="token punctuation">)</span>

    <span class="token comment"># 4. 坐标变换链</span>
    self<span class="token punctuation">.</span>transform_chain<span class="token punctuation">(</span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">euler_quaternion_conversion</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""欧拉角与四元数转换"""</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string">"=== Euler to Quaternion Conversion ==="</span><span class="token punctuation">)</span>

    <span class="token comment"># 欧拉角 (roll, pitch, yaw) - 弧度</span>
    roll <span class="token operator">=</span> <span class="token number">0.1</span>   <span class="token comment"># 绕X轴</span>
    pitch <span class="token operator">=</span> <span class="token number">0.2</span>  <span class="token comment"># 绕Y轴</span>
    yaw <span class="token operator">=</span> <span class="token number">0.3</span>    <span class="token comment"># 绕Z轴</span>

    <span class="token comment"># 转换为四元数</span>
    quaternion <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>quaternion_from_euler<span class="token punctuation">(</span>roll<span class="token punctuation">,</span> pitch<span class="token punctuation">,</span> yaw<span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Euler angles: roll=</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>roll<span class="token punctuation">:</span><span class="token format-spec">.2f</span><span class="token punctuation">}</span></span><span class="token string">, pitch=</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>pitch<span class="token punctuation">:</span><span class="token format-spec">.2f</span><span class="token punctuation">}</span></span><span class="token string">, yaw=</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>yaw<span class="token punctuation">:</span><span class="token format-spec">.2f</span><span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Quaternion: x=</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>quaternion<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, y=</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>quaternion<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span><span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, "</span></span>
          <span class="token string-interpolation"><span class="token string">f"z=</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>quaternion<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span><span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, w=</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>quaternion<span class="token punctuation">[</span><span class="token number">3</span><span class="token punctuation">]</span><span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

    <span class="token comment"># 四元数转回欧拉角</span>
    euler <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>euler_from_quaternion<span class="token punctuation">(</span>quaternion<span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Back to Euler: roll=</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>euler<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">:</span><span class="token format-spec">.2f</span><span class="token punctuation">}</span></span><span class="token string">, pitch=</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>euler<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span><span class="token punctuation">:</span><span class="token format-spec">.2f</span><span class="token punctuation">}</span></span><span class="token string">, "</span></span>
          <span class="token string-interpolation"><span class="token string">f"yaw=</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>euler<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span><span class="token punctuation">:</span><span class="token format-spec">.2f</span><span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

    <span class="token comment"># 四元数归一化（重要！）</span>
    norm <span class="token operator">=</span> np<span class="token punctuation">.</span>linalg<span class="token punctuation">.</span>norm<span class="token punctuation">(</span>quaternion<span class="token punctuation">)</span>
    quaternion_normalized <span class="token operator">=</span> quaternion <span class="token operator">/</span> norm
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Normalized quaternion norm: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>np<span class="token punctuation">.</span>linalg<span class="token punctuation">.</span>norm<span class="token punctuation">(</span>quaternion_normalized<span class="token punctuation">)</span><span class="token punctuation">:</span><span class="token format-spec">.6f</span><span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">rotation_matrix_demo</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""旋转矩阵演示"""</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string">"\n=== Rotation Matrix ==="</span><span class="token punctuation">)</span>

    <span class="token comment"># 从欧拉角创建旋转矩阵</span>
    roll<span class="token punctuation">,</span> pitch<span class="token punctuation">,</span> yaw <span class="token operator">=</span> <span class="token number">0.1</span><span class="token punctuation">,</span> <span class="token number">0.2</span><span class="token punctuation">,</span> <span class="token number">0.3</span>
    R <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>euler_matrix<span class="token punctuation">(</span>roll<span class="token punctuation">,</span> pitch<span class="token punctuation">,</span> yaw<span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Rotation matrix from Euler angles:"</span></span><span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span>R<span class="token punctuation">[</span><span class="token punctuation">:</span><span class="token number">3</span><span class="token punctuation">,</span> <span class="token punctuation">:</span><span class="token number">3</span><span class="token punctuation">]</span><span class="token punctuation">)</span>

    <span class="token comment"># 从旋转矩阵提取欧拉角</span>
    euler <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>euler_from_matrix<span class="token punctuation">(</span>R<span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Extracted Euler: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>euler<span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

    <span class="token comment"># 绕轴旋转</span>
    axis <span class="token operator">=</span> <span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">1</span><span class="token punctuation">]</span>  <span class="token comment"># Z轴</span>
    angle <span class="token operator">=</span> np<span class="token punctuation">.</span>pi <span class="token operator">/</span> <span class="token number">4</span>  <span class="token comment"># 45度</span>
    R_axis <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>rotation_matrix<span class="token punctuation">(</span>angle<span class="token punctuation">,</span> axis<span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Rotation matrix around Z-axis by 45°:"</span></span><span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span>R_axis<span class="token punctuation">[</span><span class="token punctuation">:</span><span class="token number">3</span><span class="token punctuation">,</span> <span class="token punctuation">:</span><span class="token number">3</span><span class="token punctuation">]</span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">homogeneous_transform</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""齐次变换矩阵"""</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string">"\n=== Homogeneous Transform ==="</span><span class="token punctuation">)</span>

    <span class="token comment"># 创建变换矩阵</span>
    <span class="token comment"># 平移</span>
    translation <span class="token operator">=</span> <span class="token punctuation">[</span><span class="token number">1.0</span><span class="token punctuation">,</span> <span class="token number">2.0</span><span class="token punctuation">,</span> <span class="token number">3.0</span><span class="token punctuation">]</span>

    <span class="token comment"># 旋转（四元数）</span>
    quaternion <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>quaternion_from_euler<span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> np<span class="token punctuation">.</span>pi<span class="token operator">/</span><span class="token number">4</span><span class="token punctuation">)</span>

    <span class="token comment"># 组合成齐次变换矩阵</span>
    T <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>quaternion_matrix<span class="token punctuation">(</span>quaternion<span class="token punctuation">)</span>
    T<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">:</span><span class="token number">3</span><span class="token punctuation">,</span> <span class="token number">3</span><span class="token punctuation">]</span> <span class="token operator">=</span> translation

    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Homogeneous transform matrix:"</span></span><span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span>T<span class="token punctuation">)</span>

    <span class="token comment"># 应用变换</span>
    point <span class="token operator">=</span> <span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">1</span><span class="token punctuation">]</span>  <span class="token comment"># 齐次坐标</span>
    transformed_point <span class="token operator">=</span> T @ point
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Original point: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>point<span class="token punctuation">[</span><span class="token punctuation">:</span><span class="token format-spec">3]</span><span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Transformed point: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>transformed_point<span class="token punctuation">[</span><span class="token punctuation">:</span><span class="token format-spec">3]</span><span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

    <span class="token comment"># 逆变换</span>
    T_inv <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>inverse_matrix<span class="token punctuation">(</span>T<span class="token punctuation">)</span>
    point_back <span class="token operator">=</span> T_inv @ transformed_point
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Inverse transformed: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>point_back<span class="token punctuation">[</span><span class="token punctuation">:</span><span class="token format-spec">3]</span><span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">transform_chain</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""坐标变换链"""</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string">"\n=== Transform Chain ==="</span><span class="token punctuation">)</span>

    <span class="token comment"># A到B的变换</span>
    T_AB <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>euler_matrix<span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> np<span class="token punctuation">.</span>pi<span class="token operator">/</span><span class="token number">4</span><span class="token punctuation">)</span>
    T_AB<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">:</span><span class="token number">3</span><span class="token punctuation">,</span> <span class="token number">3</span><span class="token punctuation">]</span> <span class="token operator">=</span> <span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">]</span>

    <span class="token comment"># B到C的变换</span>
    T_BC <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>euler_matrix<span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> np<span class="token punctuation">.</span>pi<span class="token operator">/</span><span class="token number">4</span><span class="token punctuation">)</span>
    T_BC<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">:</span><span class="token number">3</span><span class="token punctuation">,</span> <span class="token number">3</span><span class="token punctuation">]</span> <span class="token operator">=</span> <span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">1</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">]</span>

    <span class="token comment"># A到C的变换 = A到B * B到C</span>
    T_AC <span class="token operator">=</span> T_AB @ T_BC

    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string">"Transform A-&gt;B-&gt;C:"</span><span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Translation: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>T_AC<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">:</span><span class="token format-spec">3, 3]</span><span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

    <span class="token comment"># 从变换矩阵提取平移和旋转</span>
    translation <span class="token operator">=</span> T_AC<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">:</span><span class="token number">3</span><span class="token punctuation">,</span> <span class="token number">3</span><span class="token punctuation">]</span>
    quaternion <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>quaternion_from_matrix<span class="token punctuation">(</span>T_AC<span class="token punctuation">)</span>
    euler <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>euler_from_matrix<span class="token punctuation">(</span>T_AC<span class="token punctuation">)</span>

    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Final translation: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>translation<span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Final rotation (euler): </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>euler<span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

一键获取完整项目代码 python
运行
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129

2.2 坐标系约定
class CoordinateConventions:
    """ROS坐标系约定"""

<span class="token keyword">def</span> <span class="token function">__init__</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""
    ROS使用右手坐标系：
    - X轴：向前（红色）
    - Y轴：向左（绿色）
    - Z轴：向上（蓝色）

    旋转约定：
    - Roll：绕X轴旋转
    - Pitch：绕Y轴旋转
    - Yaw：绕Z轴旋转
    """</span>
    self<span class="token punctuation">.</span>standard_frames <span class="token operator">=</span> <span class="token punctuation">{<!-- --></span>
        <span class="token string">'map'</span><span class="token punctuation">:</span> <span class="token string">'全局固定坐标系，通常是建图的起点'</span><span class="token punctuation">,</span>
        <span class="token string">'odom'</span><span class="token punctuation">:</span> <span class="token string">'里程计坐标系，连续但会漂移'</span><span class="token punctuation">,</span>
        <span class="token string">'base_link'</span><span class="token punctuation">:</span> <span class="token string">'机器人本体坐标系'</span><span class="token punctuation">,</span>
        <span class="token string">'base_footprint'</span><span class="token punctuation">:</span> <span class="token string">'机器人在地面的投影'</span><span class="token punctuation">,</span>
        <span class="token string">'laser_link'</span><span class="token punctuation">:</span> <span class="token string">'激光雷达坐标系'</span><span class="token punctuation">,</span>
        <span class="token string">'camera_link'</span><span class="token punctuation">:</span> <span class="token string">'相机坐标系'</span><span class="token punctuation">,</span>
        <span class="token string">'imu_link'</span><span class="token punctuation">:</span> <span class="token string">'IMU坐标系'</span><span class="token punctuation">,</span>
        <span class="token string">'gps_link'</span><span class="token punctuation">:</span> <span class="token string">'GPS坐标系'</span>
    <span class="token punctuation">}</span>

<span class="token keyword">def</span> <span class="token function">print_conventions</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""打印坐标系约定"""</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string">"=== ROS Coordinate Conventions ==="</span><span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string">"Right-handed coordinate system:"</span><span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string">"  X-axis: Forward (Red)"</span><span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string">"  Y-axis: Left (Green)"</span><span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string">"  Z-axis: Up (Blue)"</span><span class="token punctuation">)</span>
    <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string">"\nStandard frames:"</span><span class="token punctuation">)</span>
    <span class="token keyword">for</span> frame<span class="token punctuation">,</span> description <span class="token keyword">in</span> self<span class="token punctuation">.</span>standard_frames<span class="token punctuation">.</span>items<span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">:</span>
        <span class="token keyword">print</span><span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"  </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>frame<span class="token punctuation">}</span></span><span class="token string">: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>description<span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

一键获取完整项目代码 python
运行
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36

3. 静态坐标变换

3.1 静态变换发布器
// 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();
}

<span class="token keyword">void</span> <span class="token function">publishStaticTransforms</span><span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span>
    <span class="token comment">// 创建静态变换广播器</span>
    <span class="token keyword">static</span> tf2_ros<span class="token operator">::</span>StaticTransformBroadcaster static_broadcaster<span class="token punctuation">;</span>

    <span class="token comment">// 1. base_link到laser_link的变换</span>
    geometry_msgs<span class="token operator">::</span>TransformStamped laser_transform<span class="token punctuation">;</span>
    laser_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> ros<span class="token operator">::</span><span class="token class-name">Time</span><span class="token operator">::</span><span class="token function">now</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    laser_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"base_link"</span><span class="token punctuation">;</span>
    laser_transform<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> <span class="token string">"laser_link"</span><span class="token punctuation">;</span>

    <span class="token comment">// 设置平移（激光雷达在机器人前方0.2m，高0.1m）</span>
    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">0.2</span><span class="token punctuation">;</span>
    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0.0</span><span class="token punctuation">;</span>
    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">0.1</span><span class="token punctuation">;</span>

    <span class="token comment">// 设置旋转（无旋转）</span>
    tf2<span class="token operator">::</span>Quaternion q<span class="token punctuation">;</span>
    q<span class="token punctuation">.</span><span class="token function">setRPY</span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>x <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>y <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>z <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>w <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">w</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>

    <span class="token comment">// 2. base_link到camera_link的变换</span>
    geometry_msgs<span class="token operator">::</span>TransformStamped camera_transform<span class="token punctuation">;</span>
    camera_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> ros<span class="token operator">::</span><span class="token class-name">Time</span><span class="token operator">::</span><span class="token function">now</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    camera_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"base_link"</span><span class="token punctuation">;</span>
    camera_transform<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> <span class="token string">"camera_link"</span><span class="token punctuation">;</span>

    <span class="token comment">// 相机在机器人前方0.15m，高0.3m，向下倾斜15度</span>
    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">0.15</span><span class="token punctuation">;</span>
    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0.0</span><span class="token punctuation">;</span>
    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">0.3</span><span class="token punctuation">;</span>

    q<span class="token punctuation">.</span><span class="token function">setRPY</span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token operator">-</span><span class="token number">0.2618</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">;</span>  <span class="token comment">// pitch = -15度</span>
    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>x <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>y <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>z <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>w <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">w</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>

    <span class="token comment">// 3. base_link到imu_link的变换</span>
    geometry_msgs<span class="token operator">::</span>TransformStamped imu_transform<span class="token punctuation">;</span>
    imu_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> ros<span class="token operator">::</span><span class="token class-name">Time</span><span class="token operator">::</span><span class="token function">now</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    imu_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"base_link"</span><span class="token punctuation">;</span>
    imu_transform<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> <span class="token string">"imu_link"</span><span class="token punctuation">;</span>

    imu_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">0.0</span><span class="token punctuation">;</span>
    imu_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0.0</span><span class="token punctuation">;</span>
    imu_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">0.05</span><span class="token punctuation">;</span>

    <span class="token comment">// IMU可能有安装角度偏差</span>
    q<span class="token punctuation">.</span><span class="token function">setRPY</span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    imu_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>x <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    imu_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>y <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    imu_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>z <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    imu_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>w <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">w</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>

    <span class="token comment">// 发布所有静态变换</span>
    std<span class="token operator">::</span>vector<span class="token operator">&lt;</span>geometry_msgs<span class="token operator">::</span>TransformStamped<span class="token operator">&gt;</span> static_transforms<span class="token punctuation">;</span>
    static_transforms<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>laser_transform<span class="token punctuation">)</span><span class="token punctuation">;</span>
    static_transforms<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>camera_transform<span class="token punctuation">)</span><span class="token punctuation">;</span>
    static_transforms<span class="token punctuation">.</span><span class="token function">push_back</span><span class="token punctuation">(</span>imu_transform<span class="token punctuation">)</span><span class="token punctuation">;</span>

    static_broadcaster<span class="token punctuation">.</span><span class="token function">sendTransform</span><span class="token punctuation">(</span>static_transforms<span class="token punctuation">)</span><span class="token punctuation">;</span>

    <span class="token function">ROS_INFO</span><span class="token punctuation">(</span><span class="token string">"Published static transforms"</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>

private:
ros::NodeHandle nh_;
};

int main(int argc, char argv) {
ros::init(argc, argv, “static_transform_publisher”);
StaticTransformPublisher publisher;
ros::spin();
return 0;
}

一键获取完整项目代码 cpp
运行
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91

3.2 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:
“”“静态坐标变换发布器”“”

<span class="token keyword">def</span> <span class="token function">__init__</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    rospy<span class="token punctuation">.</span>init_node<span class="token punctuation">(</span><span class="token string">'static_transform_publisher'</span><span class="token punctuation">)</span>

    <span class="token comment"># 创建静态变换广播器</span>
    self<span class="token punctuation">.</span>static_broadcaster <span class="token operator">=</span> tf2_ros<span class="token punctuation">.</span>StaticTransformBroadcaster<span class="token punctuation">(</span><span class="token punctuation">)</span>

    <span class="token comment"># 发布静态变换</span>
    self<span class="token punctuation">.</span>publish_static_transforms<span class="token punctuation">(</span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">publish_static_transforms</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""发布静态变换"""</span>

    static_transforms <span class="token operator">=</span> <span class="token punctuation">[</span><span class="token punctuation">]</span>

    <span class="token comment"># 1. base_link到laser_link</span>
    laser_transform <span class="token operator">=</span> TransformStamped<span class="token punctuation">(</span><span class="token punctuation">)</span>
    laser_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> rospy<span class="token punctuation">.</span>Time<span class="token punctuation">.</span>now<span class="token punctuation">(</span><span class="token punctuation">)</span>
    laser_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"base_link"</span>
    laser_transform<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> <span class="token string">"laser_link"</span>

    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">0.2</span>
    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0.0</span>
    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">0.1</span>

    q <span class="token operator">=</span> tf_trans<span class="token punctuation">.</span>quaternion_from_euler<span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">)</span>
    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>x <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span>
    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>y <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span>
    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>z <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span>
    laser_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>w <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">3</span><span class="token punctuation">]</span>

    static_transforms<span class="token punctuation">.</span>append<span class="token punctuation">(</span>laser_transform<span class="token punctuation">)</span>

    <span class="token comment"># 2. base_link到camera_link</span>
    camera_transform <span class="token operator">=</span> TransformStamped<span class="token punctuation">(</span><span class="token punctuation">)</span>
    camera_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> rospy<span class="token punctuation">.</span>Time<span class="token punctuation">.</span>now<span class="token punctuation">(</span><span class="token punctuation">)</span>
    camera_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"base_link"</span>
    camera_transform<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> <span class="token string">"camera_link"</span>

    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">0.15</span>
    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0.0</span>
    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">0.3</span>

    <span class="token comment"># 相机向下倾斜15度</span>
    q <span class="token operator">=</span> tf_trans<span class="token punctuation">.</span>quaternion_from_euler<span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token operator">-</span><span class="token number">0.2618</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">)</span>
    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>x <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span>
    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>y <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span>
    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>z <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span>
    camera_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>w <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">3</span><span class="token punctuation">]</span>

    static_transforms<span class="token punctuation">.</span>append<span class="token punctuation">(</span>camera_transform<span class="token punctuation">)</span>

    <span class="token comment"># 3. base_link到多个传感器</span>
    sensor_configs <span class="token operator">=</span> <span class="token punctuation">[</span>
        <span class="token punctuation">{<!-- --></span><span class="token string">'name'</span><span class="token punctuation">:</span> <span class="token string">'imu_link'</span><span class="token punctuation">,</span> <span class="token string">'x'</span><span class="token punctuation">:</span> <span class="token number">0.0</span><span class="token punctuation">,</span> <span class="token string">'y'</span><span class="token punctuation">:</span> <span class="token number">0.0</span><span class="token punctuation">,</span> <span class="token string">'z'</span><span class="token punctuation">:</span> <span class="token number">0.05</span><span class="token punctuation">,</span> <span class="token string">'roll'</span><span class="token punctuation">:</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token string">'pitch'</span><span class="token punctuation">:</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token string">'yaw'</span><span class="token punctuation">:</span> <span class="token number">0</span><span class="token punctuation">}</span><span class="token punctuation">,</span>
        <span class="token punctuation">{<!-- --></span><span class="token string">'name'</span><span class="token punctuation">:</span> <span class="token string">'gps_link'</span><span class="token punctuation">,</span> <span class="token string">'x'</span><span class="token punctuation">:</span> <span class="token operator">-</span><span class="token number">0.1</span><span class="token punctuation">,</span> <span class="token string">'y'</span><span class="token punctuation">:</span> <span class="token number">0.0</span><span class="token punctuation">,</span> <span class="token string">'z'</span><span class="token punctuation">:</span> <span class="token number">0.2</span><span class="token punctuation">,</span> <span class="token string">'roll'</span><span class="token punctuation">:</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token string">'pitch'</span><span class="token punctuation">:</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token string">'yaw'</span><span class="token punctuation">:</span> <span class="token number">0</span><span class="token punctuation">}</span><span class="token punctuation">,</span>
        <span class="token punctuation">{<!-- --></span><span class="token string">'name'</span><span class="token punctuation">:</span> <span class="token string">'ultrasonic_front'</span><span class="token punctuation">,</span> <span class="token string">'x'</span><span class="token punctuation">:</span> <span class="token number">0.25</span><span class="token punctuation">,</span> <span class="token string">'y'</span><span class="token punctuation">:</span> <span class="token number">0.0</span><span class="token punctuation">,</span> <span class="token string">'z'</span><span class="token punctuation">:</span> <span class="token number">0.05</span><span class="token punctuation">,</span> <span class="token string">'roll'</span><span class="token punctuation">:</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token string">'pitch'</span><span class="token punctuation">:</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token string">'yaw'</span><span class="token punctuation">:</span> <span class="token number">0</span><span class="token punctuation">}</span><span class="token punctuation">,</span>
        <span class="token punctuation">{<!-- --></span><span class="token string">'name'</span><span class="token punctuation">:</span> <span class="token string">'ultrasonic_back'</span><span class="token punctuation">,</span> <span class="token string">'x'</span><span class="token punctuation">:</span> <span class="token operator">-</span><span class="token number">0.25</span><span class="token punctuation">,</span> <span class="token string">'y'</span><span class="token punctuation">:</span> <span class="token number">0.0</span><span class="token punctuation">,</span> <span class="token string">'z'</span><span class="token punctuation">:</span> <span class="token number">0.05</span><span class="token punctuation">,</span> <span class="token string">'roll'</span><span class="token punctuation">:</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token string">'pitch'</span><span class="token punctuation">:</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token string">'yaw'</span><span class="token punctuation">:</span> <span class="token number">3.14159</span><span class="token punctuation">}</span><span class="token punctuation">,</span>
    <span class="token punctuation">]</span>

    <span class="token keyword">for</span> config <span class="token keyword">in</span> sensor_configs<span class="token punctuation">:</span>
        transform <span class="token operator">=</span> self<span class="token punctuation">.</span>create_transform<span class="token punctuation">(</span>
            <span class="token string">"base_link"</span><span class="token punctuation">,</span>
            config<span class="token punctuation">[</span><span class="token string">'name'</span><span class="token punctuation">]</span><span class="token punctuation">,</span>
            config<span class="token punctuation">[</span><span class="token string">'x'</span><span class="token punctuation">]</span><span class="token punctuation">,</span> config<span class="token punctuation">[</span><span class="token string">'y'</span><span class="token punctuation">]</span><span class="token punctuation">,</span> config<span class="token punctuation">[</span><span class="token string">'z'</span><span class="token punctuation">]</span><span class="token punctuation">,</span>
            config<span class="token punctuation">[</span><span class="token string">'roll'</span><span class="token punctuation">]</span><span class="token punctuation">,</span> config<span class="token punctuation">[</span><span class="token string">'pitch'</span><span class="token punctuation">]</span><span class="token punctuation">,</span> config<span class="token punctuation">[</span><span class="token string">'yaw'</span><span class="token punctuation">]</span>
        <span class="token punctuation">)</span>
        static_transforms<span class="token punctuation">.</span>append<span class="token punctuation">(</span>transform<span class="token punctuation">)</span>

    <span class="token comment"># 发布所有静态变换</span>
    self<span class="token punctuation">.</span>static_broadcaster<span class="token punctuation">.</span>sendTransform<span class="token punctuation">(</span>static_transforms<span class="token punctuation">)</span>
    rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Published </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span><span class="token builtin">len</span><span class="token punctuation">(</span>static_transforms<span class="token punctuation">)</span><span class="token punctuation">}</span></span><span class="token string"> static transforms"</span></span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">create_transform</span><span class="token punctuation">(</span>self<span class="token punctuation">,</span> parent_frame<span class="token punctuation">,</span> child_frame<span class="token punctuation">,</span> x<span class="token punctuation">,</span> y<span class="token punctuation">,</span> z<span class="token punctuation">,</span> roll<span class="token punctuation">,</span> pitch<span class="token punctuation">,</span> yaw<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""创建变换消息"""</span>
    transform <span class="token operator">=</span> TransformStamped<span class="token punctuation">(</span><span class="token punctuation">)</span>

    transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> rospy<span class="token punctuation">.</span>Time<span class="token punctuation">.</span>now<span class="token punctuation">(</span><span class="token punctuation">)</span>
    transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> parent_frame
    transform<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> child_frame

    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> x
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> y
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> z

    q <span class="token operator">=</span> tf_trans<span class="token punctuation">.</span>quaternion_from_euler<span class="token punctuation">(</span>roll<span class="token punctuation">,</span> pitch<span class="token punctuation">,</span> yaw<span class="token punctuation">)</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>x <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>y <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>z <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>w <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">3</span><span class="token punctuation">]</span>

    <span class="token keyword">return</span> transform

<span class="token keyword">def</span> <span class="token function">publish_from_urdf</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""从URDF参数发布静态变换"""</span>
    <span class="token comment"># 这通常由robot_state_publisher完成</span>
    <span class="token comment"># 但我们可以手动解析URDF并发布</span>

    <span class="token keyword">if</span> rospy<span class="token punctuation">.</span>has_param<span class="token punctuation">(</span><span class="token string">'robot_description'</span><span class="token punctuation">)</span><span class="token punctuation">:</span>
        robot_description <span class="token operator">=</span> rospy<span class="token punctuation">.</span>get_param<span class="token punctuation">(</span><span class="token string">'robot_description'</span><span class="token punctuation">)</span>
        <span class="token comment"># 解析URDF并提取固定关节的变换</span>
        <span class="token comment"># 这里简化处理</span>
        rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string">"Would parse URDF and publish fixed joints"</span><span class="token punctuation">)</span>

def main():
publisher = StaticTransformPublisher()
rospy.spin()

if name == ‘main’:
main()

一键获取完整项目代码 python
运行
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120

3.3 Launch文件配置静态变换
<!-- 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>

一键获取完整项目代码 xml
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38

4. 动态坐标变换

4.1 动态变换广播器
// 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>();

    <span class="token comment">// 订阅里程计</span>
    odom_sub_ <span class="token operator">=</span> nh_<span class="token punctuation">.</span><span class="token function">subscribe</span><span class="token punctuation">(</span><span class="token string">"/odom"</span><span class="token punctuation">,</span> <span class="token number">10</span><span class="token punctuation">,</span>
        <span class="token operator">&amp;</span>DynamicTransformBroadcaster<span class="token operator">::</span>odomCallback<span class="token punctuation">,</span> <span class="token keyword">this</span><span class="token punctuation">)</span><span class="token punctuation">;</span>

    <span class="token comment">// 订阅关节状态</span>
    joint_sub_ <span class="token operator">=</span> nh_<span class="token punctuation">.</span><span class="token function">subscribe</span><span class="token punctuation">(</span><span class="token string">"/joint_states"</span><span class="token punctuation">,</span> <span class="token number">10</span><span class="token punctuation">,</span>
        <span class="token operator">&amp;</span>DynamicTransformBroadcaster<span class="token operator">::</span>jointCallback<span class="token punctuation">,</span> <span class="token keyword">this</span><span class="token punctuation">)</span><span class="token punctuation">;</span>

    <span class="token comment">// 定时发布变换</span>
    timer_ <span class="token operator">=</span> nh_<span class="token punctuation">.</span><span class="token function">createTimer</span><span class="token punctuation">(</span>ros<span class="token operator">::</span><span class="token function">Duration</span><span class="token punctuation">(</span><span class="token number">0.02</span><span class="token punctuation">)</span><span class="token punctuation">,</span>  <span class="token comment">// 50Hz</span>
        <span class="token operator">&amp;</span>DynamicTransformBroadcaster<span class="token operator">::</span>timerCallback<span class="token punctuation">,</span> <span class="token keyword">this</span><span class="token punctuation">)</span><span class="token punctuation">;</span>

    <span class="token function">ROS_INFO</span><span class="token punctuation">(</span><span class="token string">"Dynamic transform broadcaster initialized"</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>

private:
void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
// 发布odom到base_link的变换
geometry_msgs::TransformStamped odom_transform;

    odom_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> msg<span class="token operator">-&gt;</span>header<span class="token punctuation">.</span>stamp<span class="token punctuation">;</span>
    odom_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"odom"</span><span class="token punctuation">;</span>
    odom_transform<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> <span class="token string">"base_link"</span><span class="token punctuation">;</span>

    <span class="token comment">// 从里程计消息复制位置</span>
    odom_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> msg<span class="token operator">-&gt;</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>x<span class="token punctuation">;</span>
    odom_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> msg<span class="token operator">-&gt;</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>y<span class="token punctuation">;</span>
    odom_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> msg<span class="token operator">-&gt;</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>z<span class="token punctuation">;</span>

    <span class="token comment">// 从里程计消息复制姿态</span>
    odom_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation <span class="token operator">=</span> msg<span class="token operator">-&gt;</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>orientation<span class="token punctuation">;</span>

    <span class="token comment">// 发送变换</span>
    tf_broadcaster_<span class="token operator">-&gt;</span><span class="token function">sendTransform</span><span class="token punctuation">(</span>odom_transform<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>

<span class="token keyword">void</span> <span class="token function">jointCallback</span><span class="token punctuation">(</span><span class="token keyword">const</span> sensor_msgs<span class="token operator">::</span>JointState<span class="token operator">::</span>ConstPtr<span class="token operator">&amp;</span> msg<span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span>
    <span class="token comment">// 发布关节变换（例如机械臂）</span>
    <span class="token keyword">for</span> <span class="token punctuation">(</span>size_t i <span class="token operator">=</span> <span class="token number">0</span><span class="token punctuation">;</span> i <span class="token operator">&lt;</span> msg<span class="token operator">-&gt;</span>name<span class="token punctuation">.</span><span class="token function">size</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span> <span class="token operator">++</span>i<span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span>
        <span class="token keyword">if</span> <span class="token punctuation">(</span>msg<span class="token operator">-&gt;</span>name<span class="token punctuation">[</span>i<span class="token punctuation">]</span> <span class="token operator">==</span> <span class="token string">"arm_joint_1"</span><span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span>
            <span class="token function">publishJointTransform</span><span class="token punctuation">(</span><span class="token string">"arm_base_link"</span><span class="token punctuation">,</span> <span class="token string">"arm_link_1"</span><span class="token punctuation">,</span>
                <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0.1</span><span class="token punctuation">,</span>  <span class="token comment">// 平移</span>
                msg<span class="token operator">-&gt;</span>position<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">;</span>  <span class="token comment">// 旋转（绕Z轴）</span>
        <span class="token punctuation">}</span>
        <span class="token keyword">else</span> <span class="token keyword">if</span> <span class="token punctuation">(</span>msg<span class="token operator">-&gt;</span>name<span class="token punctuation">[</span>i<span class="token punctuation">]</span> <span class="token operator">==</span> <span class="token string">"arm_joint_2"</span><span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span>
            <span class="token function">publishJointTransform</span><span class="token punctuation">(</span><span class="token string">"arm_link_1"</span><span class="token punctuation">,</span> <span class="token string">"arm_link_2"</span><span class="token punctuation">,</span>
                <span class="token number">0.2</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span>  <span class="token comment">// 平移</span>
                <span class="token number">0</span><span class="token punctuation">,</span> msg<span class="token operator">-&gt;</span>position<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">)</span><span class="token punctuation">;</span>  <span class="token comment">// 旋转（绕Y轴）</span>
        <span class="token punctuation">}</span>
        <span class="token comment">// ... 更多关节</span>
    <span class="token punctuation">}</span>
<span class="token punctuation">}</span>

<span class="token keyword">void</span> <span class="token function">timerCallback</span><span class="token punctuation">(</span><span class="token keyword">const</span> ros<span class="token operator">::</span>TimerEvent<span class="token operator">&amp;</span> event<span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span>
    <span class="token comment">// 发布时变的变换（例如旋转的传感器）</span>
    ros<span class="token operator">::</span>Time now <span class="token operator">=</span> ros<span class="token operator">::</span><span class="token class-name">Time</span><span class="token operator">::</span><span class="token function">now</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>

    <span class="token comment">// 旋转的激光雷达</span>
    <span class="token keyword">double</span> angle <span class="token operator">=</span> <span class="token number">2</span> <span class="token operator">*</span> M_PI <span class="token operator">*</span> <span class="token function">fmod</span><span class="token punctuation">(</span>now<span class="token punctuation">.</span><span class="token function">toSec</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">,</span> <span class="token number">10.0</span><span class="token punctuation">)</span> <span class="token operator">/</span> <span class="token number">10.0</span><span class="token punctuation">;</span>  <span class="token comment">// 10秒一圈</span>

    geometry_msgs<span class="token operator">::</span>TransformStamped rotating_transform<span class="token punctuation">;</span>
    rotating_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> now<span class="token punctuation">;</span>
    rotating_transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"base_link"</span><span class="token punctuation">;</span>
    rotating_transform<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> <span class="token string">"rotating_laser"</span><span class="token punctuation">;</span>

    rotating_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">0.0</span><span class="token punctuation">;</span>
    rotating_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0.0</span><span class="token punctuation">;</span>
    rotating_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">0.2</span><span class="token punctuation">;</span>

    tf2<span class="token operator">::</span>Quaternion q<span class="token punctuation">;</span>
    q<span class="token punctuation">.</span><span class="token function">setRPY</span><span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> angle<span class="token punctuation">)</span><span class="token punctuation">;</span>
    rotating_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>x <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    rotating_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>y <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    rotating_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>z <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    rotating_transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>w <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">w</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>

    tf_broadcaster_<span class="token operator">-&gt;</span><span class="token function">sendTransform</span><span class="token punctuation">(</span>rotating_transform<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>

<span class="token keyword">void</span> <span class="token function">publishJointTransform</span><span class="token punctuation">(</span><span class="token keyword">const</span> std<span class="token operator">::</span>string<span class="token operator">&amp;</span> parent_frame<span class="token punctuation">,</span>
                           <span class="token keyword">const</span> std<span class="token operator">::</span>string<span class="token operator">&amp;</span> child_frame<span class="token punctuation">,</span>
                           <span class="token keyword">double</span> x<span class="token punctuation">,</span> <span class="token keyword">double</span> y<span class="token punctuation">,</span> <span class="token keyword">double</span> z<span class="token punctuation">,</span>
                           <span class="token keyword">double</span> roll<span class="token punctuation">,</span> <span class="token keyword">double</span> pitch<span class="token punctuation">,</span> <span class="token keyword">double</span> yaw<span class="token punctuation">)</span> <span class="token punctuation">{<!-- --></span>
    geometry_msgs<span class="token operator">::</span>TransformStamped transform<span class="token punctuation">;</span>

    transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> ros<span class="token operator">::</span><span class="token class-name">Time</span><span class="token operator">::</span><span class="token function">now</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> parent_frame<span class="token punctuation">;</span>
    transform<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> child_frame<span class="token punctuation">;</span>

    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> x<span class="token punctuation">;</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> y<span class="token punctuation">;</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> z<span class="token punctuation">;</span>

    tf2<span class="token operator">::</span>Quaternion q<span class="token punctuation">;</span>
    q<span class="token punctuation">.</span><span class="token function">setRPY</span><span class="token punctuation">(</span>roll<span class="token punctuation">,</span> pitch<span class="token punctuation">,</span> yaw<span class="token punctuation">)</span><span class="token punctuation">;</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>x <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>y <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>z <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>w <span class="token operator">=</span> q<span class="token punctuation">.</span><span class="token function">w</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>

    tf_broadcaster_<span class="token operator">-&gt;</span><span class="token function">sendTransform</span><span class="token punctuation">(</span>transform<span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>

ros<span class="token operator">::</span>NodeHandle nh_<span class="token punctuation">;</span>
std<span class="token operator">::</span>unique_ptr<span class="token operator">&lt;</span>tf2_ros<span class="token operator">::</span>TransformBroadcaster<span class="token operator">&gt;</span> tf_broadcaster_<span class="token punctuation">;</span>
ros<span class="token operator">::</span>Subscriber odom_sub_<span class="token punctuation">;</span>
ros<span class="token operator">::</span>Subscriber joint_sub_<span class="token punctuation">;</span>
ros<span class="token operator">::</span>Timer timer_<span class="token punctuation">;</span>

};

int main(int argc, char argv) {
ros::init(argc, argv, “dynamic_transform_broadcaster”);
DynamicTransformBroadcaster broadcaster;
ros::spin();
return 0;
}

一键获取完整项目代码 cpp
运行
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130

4.2 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:
“”“动态坐标变换广播器”“”

<span class="token keyword">def</span> <span class="token function">__init__</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    rospy<span class="token punctuation">.</span>init_node<span class="token punctuation">(</span><span class="token string">'dynamic_transform_broadcaster'</span><span class="token punctuation">)</span>

    <span class="token comment"># TF广播器</span>
    self<span class="token punctuation">.</span>tf_broadcaster <span class="token operator">=</span> tf2_ros<span class="token punctuation">.</span>TransformBroadcaster<span class="token punctuation">(</span><span class="token punctuation">)</span>

    <span class="token comment"># 订阅者</span>
    self<span class="token punctuation">.</span>odom_sub <span class="token operator">=</span> rospy<span class="token punctuation">.</span>Subscriber<span class="token punctuation">(</span>
        <span class="token string">'/odom'</span><span class="token punctuation">,</span> Odometry<span class="token punctuation">,</span> self<span class="token punctuation">.</span>odom_callback<span class="token punctuation">)</span>
    self<span class="token punctuation">.</span>joint_sub <span class="token operator">=</span> rospy<span class="token punctuation">.</span>Subscriber<span class="token punctuation">(</span>
        <span class="token string">'/joint_states'</span><span class="token punctuation">,</span> JointState<span class="token punctuation">,</span> self<span class="token punctuation">.</span>joint_callback<span class="token punctuation">)</span>

    <span class="token comment"># 定时器</span>
    self<span class="token punctuation">.</span>timer <span class="token operator">=</span> rospy<span class="token punctuation">.</span>Timer<span class="token punctuation">(</span>rospy<span class="token punctuation">.</span>Duration<span class="token punctuation">(</span><span class="token number">0.02</span><span class="token punctuation">)</span><span class="token punctuation">,</span> self<span class="token punctuation">.</span>timer_callback<span class="token punctuation">)</span>

    <span class="token comment"># 状态</span>
    self<span class="token punctuation">.</span>robot_x <span class="token operator">=</span> <span class="token number">0.0</span>
    self<span class="token punctuation">.</span>robot_y <span class="token operator">=</span> <span class="token number">0.0</span>
    self<span class="token punctuation">.</span>robot_theta <span class="token operator">=</span> <span class="token number">0.0</span>

    rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string">"Dynamic transform broadcaster started"</span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">odom_callback</span><span class="token punctuation">(</span>self<span class="token punctuation">,</span> msg<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""里程计回调"""</span>
    <span class="token comment"># 发布odom到base_link的变换</span>
    transform <span class="token operator">=</span> TransformStamped<span class="token punctuation">(</span><span class="token punctuation">)</span>

    transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> msg<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp
    transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"odom"</span>
    transform<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> <span class="token string">"base_link"</span>

    <span class="token comment"># 位置</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> msg<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>x
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> msg<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>y
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> msg<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>z

    <span class="token comment"># 姿态</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation <span class="token operator">=</span> msg<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>orientation

    <span class="token comment"># 广播变换</span>
    self<span class="token punctuation">.</span>tf_broadcaster<span class="token punctuation">.</span>sendTransform<span class="token punctuation">(</span>transform<span class="token punctuation">)</span>

    <span class="token comment"># 更新机器人位置（用于其他计算）</span>
    self<span class="token punctuation">.</span>robot_x <span class="token operator">=</span> msg<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>x
    self<span class="token punctuation">.</span>robot_y <span class="token operator">=</span> msg<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>y

    quaternion <span class="token operator">=</span> <span class="token punctuation">(</span>
        msg<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>orientation<span class="token punctuation">.</span>x<span class="token punctuation">,</span>
        msg<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>orientation<span class="token punctuation">.</span>y<span class="token punctuation">,</span>
        msg<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>orientation<span class="token punctuation">.</span>z<span class="token punctuation">,</span>
        msg<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>orientation<span class="token punctuation">.</span>w
    <span class="token punctuation">)</span>
    euler <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>euler_from_quaternion<span class="token punctuation">(</span>quaternion<span class="token punctuation">)</span>
    self<span class="token punctuation">.</span>robot_theta <span class="token operator">=</span> euler<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span>

<span class="token keyword">def</span> <span class="token function">joint_callback</span><span class="token punctuation">(</span>self<span class="token punctuation">,</span> msg<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""关节状态回调"""</span>
    <span class="token comment"># 发布关节变换</span>
    <span class="token keyword">for</span> i<span class="token punctuation">,</span> joint_name <span class="token keyword">in</span> <span class="token builtin">enumerate</span><span class="token punctuation">(</span>msg<span class="token punctuation">.</span>name<span class="token punctuation">)</span><span class="token punctuation">:</span>
        <span class="token keyword">if</span> joint_name <span class="token operator">==</span> <span class="token string">"arm_joint_1"</span><span class="token punctuation">:</span>
            self<span class="token punctuation">.</span>publish_joint_transform<span class="token punctuation">(</span>
                <span class="token string">"arm_base_link"</span><span class="token punctuation">,</span> <span class="token string">"arm_link_1"</span><span class="token punctuation">,</span>
                <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0.1</span><span class="token punctuation">,</span>  <span class="token comment"># 平移</span>
                msg<span class="token punctuation">.</span>position<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span>  <span class="token comment"># 旋转</span>
            <span class="token punctuation">)</span>
        <span class="token keyword">elif</span> joint_name <span class="token operator">==</span> <span class="token string">"arm_joint_2"</span><span class="token punctuation">:</span>
            self<span class="token punctuation">.</span>publish_joint_transform<span class="token punctuation">(</span>
                <span class="token string">"arm_link_1"</span><span class="token punctuation">,</span> <span class="token string">"arm_link_2"</span><span class="token punctuation">,</span>
                <span class="token number">0.2</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span>  <span class="token comment"># 平移</span>
                <span class="token number">0</span><span class="token punctuation">,</span> msg<span class="token punctuation">.</span>position<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">,</span> <span class="token number">0</span>  <span class="token comment"># 旋转</span>
            <span class="token punctuation">)</span>
        <span class="token keyword">elif</span> joint_name <span class="token operator">==</span> <span class="token string">"gripper_joint"</span><span class="token punctuation">:</span>
            <span class="token comment"># 夹爪（平移关节）</span>
            self<span class="token punctuation">.</span>publish_joint_transform<span class="token punctuation">(</span>
                <span class="token string">"arm_link_2"</span><span class="token punctuation">,</span> <span class="token string">"gripper_link"</span><span class="token punctuation">,</span>
                msg<span class="token punctuation">.</span>position<span class="token punctuation">[</span>i<span class="token punctuation">]</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span>  <span class="token comment"># 平移</span>
                <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span>  <span class="token comment"># 无旋转</span>
            <span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">timer_callback</span><span class="token punctuation">(</span>self<span class="token punctuation">,</span> event<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""定时器回调"""</span>
    current_time <span class="token operator">=</span> rospy<span class="token punctuation">.</span>Time<span class="token punctuation">.</span>now<span class="token punctuation">(</span><span class="token punctuation">)</span>

    <span class="token comment"># 1. 发布旋转的传感器</span>
    self<span class="token punctuation">.</span>publish_rotating_sensor<span class="token punctuation">(</span>current_time<span class="token punctuation">)</span>

    <span class="token comment"># 2. 发布摆动的天线</span>
    self<span class="token punctuation">.</span>publish_swinging_antenna<span class="token punctuation">(</span>current_time<span class="token punctuation">)</span>

    <span class="token comment"># 3. 发布模拟的轮子旋转</span>
    self<span class="token punctuation">.</span>publish_wheel_rotation<span class="token punctuation">(</span>current_time<span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">publish_rotating_sensor</span><span class="token punctuation">(</span>self<span class="token punctuation">,</span> timestamp<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""发布旋转的传感器"""</span>
    <span class="token comment"># 10秒转一圈</span>
    period <span class="token operator">=</span> <span class="token number">10.0</span>
    angle <span class="token operator">=</span> <span class="token number">2</span> <span class="token operator">*</span> math<span class="token punctuation">.</span>pi <span class="token operator">*</span> <span class="token punctuation">(</span>timestamp<span class="token punctuation">.</span>to_sec<span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">%</span> period<span class="token punctuation">)</span> <span class="token operator">/</span> period

    transform <span class="token operator">=</span> TransformStamped<span class="token punctuation">(</span><span class="token punctuation">)</span>
    transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> timestamp
    transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"base_link"</span>
    transform<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> <span class="token string">"rotating_sensor"</span>

    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">0.0</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0.0</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">0.3</span>

    q <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>quaternion_from_euler<span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> angle<span class="token punctuation">)</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>x <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>y <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>z <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>w <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">3</span><span class="token punctuation">]</span>

    self<span class="token punctuation">.</span>tf_broadcaster<span class="token punctuation">.</span>sendTransform<span class="token punctuation">(</span>transform<span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">publish_swinging_antenna</span><span class="token punctuation">(</span>self<span class="token punctuation">,</span> timestamp<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""发布摆动的天线"""</span>
    <span class="token comment"># 正弦摆动</span>
    amplitude <span class="token operator">=</span> <span class="token number">0.5</span>  <span class="token comment"># 弧度</span>
    frequency <span class="token operator">=</span> <span class="token number">0.5</span>  <span class="token comment"># Hz</span>
    angle <span class="token operator">=</span> amplitude <span class="token operator">*</span> math<span class="token punctuation">.</span>sin<span class="token punctuation">(</span><span class="token number">2</span> <span class="token operator">*</span> math<span class="token punctuation">.</span>pi <span class="token operator">*</span> frequency <span class="token operator">*</span> timestamp<span class="token punctuation">.</span>to_sec<span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span>

    transform <span class="token operator">=</span> TransformStamped<span class="token punctuation">(</span><span class="token punctuation">)</span>
    transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> timestamp
    transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"base_link"</span>
    transform<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> <span class="token string">"antenna"</span>

    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token operator">-</span><span class="token number">0.1</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0.0</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">0.4</span>

    q <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>quaternion_from_euler<span class="token punctuation">(</span>angle<span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">)</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>x <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>y <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>z <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>w <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">3</span><span class="token punctuation">]</span>

    self<span class="token punctuation">.</span>tf_broadcaster<span class="token punctuation">.</span>sendTransform<span class="token punctuation">(</span>transform<span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">publish_wheel_rotation</span><span class="token punctuation">(</span>self<span class="token punctuation">,</span> timestamp<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""发布轮子旋转"""</span>
    <span class="token comment"># 基于机器人速度估算轮子转速</span>
    wheel_radius <span class="token operator">=</span> <span class="token number">0.05</span>  <span class="token comment"># 轮子半径</span>
    linear_velocity <span class="token operator">=</span> math<span class="token punctuation">.</span>sqrt<span class="token punctuation">(</span>self<span class="token punctuation">.</span>robot_x<span class="token operator">**</span><span class="token number">2</span> <span class="token operator">+</span> self<span class="token punctuation">.</span>robot_y<span class="token operator">**</span><span class="token number">2</span><span class="token punctuation">)</span> <span class="token operator">/</span> timestamp<span class="token punctuation">.</span>to_sec<span class="token punctuation">(</span><span class="token punctuation">)</span>

    <span class="token keyword">if</span> timestamp<span class="token punctuation">.</span>to_sec<span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">&gt;</span> <span class="token number">0</span><span class="token punctuation">:</span>
        wheel_angle <span class="token operator">=</span> linear_velocity <span class="token operator">*</span> timestamp<span class="token punctuation">.</span>to_sec<span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">/</span> wheel_radius
    <span class="token keyword">else</span><span class="token punctuation">:</span>
        wheel_angle <span class="token operator">=</span> <span class="token number">0</span>

    <span class="token comment"># 左轮</span>
    transform_left <span class="token operator">=</span> TransformStamped<span class="token punctuation">(</span><span class="token punctuation">)</span>
    transform_left<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> timestamp
    transform_left<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"base_link"</span>
    transform_left<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> <span class="token string">"wheel_left"</span>

    transform_left<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">0.0</span>
    transform_left<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0.15</span>
    transform_left<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token operator">-</span><span class="token number">0.05</span>

    q <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>quaternion_from_euler<span class="token punctuation">(</span><span class="token number">0</span><span class="token punctuation">,</span> wheel_angle<span class="token punctuation">,</span> <span class="token number">0</span><span class="token punctuation">)</span>
    transform_left<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>x <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span>
    transform_left<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>y <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span>
    transform_left<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>z <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span>
    transform_left<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>w <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">3</span><span class="token punctuation">]</span>

    <span class="token comment"># 右轮</span>
    transform_right <span class="token operator">=</span> TransformStamped<span class="token punctuation">(</span><span class="token punctuation">)</span>
    transform_right<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> timestamp
    transform_right<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"base_link"</span>
    transform_right<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> <span class="token string">"wheel_right"</span>

    transform_right<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">0.0</span>
    transform_right<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token operator">-</span><span class="token number">0.15</span>
    transform_right<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token operator">-</span><span class="token number">0.05</span>

    transform_right<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation <span class="token operator">=</span> transform_left<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation

    self<span class="token punctuation">.</span>tf_broadcaster<span class="token punctuation">.</span>sendTransform<span class="token punctuation">(</span><span class="token punctuation">[</span>transform_left<span class="token punctuation">,</span> transform_right<span class="token punctuation">]</span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">publish_joint_transform</span><span class="token punctuation">(</span>self<span class="token punctuation">,</span> parent_frame<span class="token punctuation">,</span> child_frame<span class="token punctuation">,</span>
                           x<span class="token punctuation">,</span> y<span class="token punctuation">,</span> z<span class="token punctuation">,</span> roll<span class="token punctuation">,</span> pitch<span class="token punctuation">,</span> yaw<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""发布关节变换"""</span>
    transform <span class="token operator">=</span> TransformStamped<span class="token punctuation">(</span><span class="token punctuation">)</span>

    transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> rospy<span class="token punctuation">.</span>Time<span class="token punctuation">.</span>now<span class="token punctuation">(</span><span class="token punctuation">)</span>
    transform<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> parent_frame
    transform<span class="token punctuation">.</span>child_frame_id <span class="token operator">=</span> child_frame

    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x <span class="token operator">=</span> x
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y <span class="token operator">=</span> y
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z <span class="token operator">=</span> z

    q <span class="token operator">=</span> tf<span class="token punctuation">.</span>transformations<span class="token punctuation">.</span>quaternion_from_euler<span class="token punctuation">(</span>roll<span class="token punctuation">,</span> pitch<span class="token punctuation">,</span> yaw<span class="token punctuation">)</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>x <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>y <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>z <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span>
    transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>w <span class="token operator">=</span> q<span class="token punctuation">[</span><span class="token number">3</span><span class="token punctuation">]</span>

    self<span class="token punctuation">.</span>tf_broadcaster<span class="token punctuation">.</span>sendTransform<span class="token punctuation">(</span>transform<span class="token punctuation">)</span>

def main():
broadcaster = DynamicTransformBroadcaster()
rospy.spin()

if name == ‘main’:
main()

一键获取完整项目代码 python
运行
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134
  • 135
  • 136
  • 137
  • 138
  • 139
  • 140
  • 141
  • 142
  • 143
  • 144
  • 145
  • 146
  • 147
  • 148
  • 149
  • 150
  • 151
  • 152
  • 153
  • 154
  • 155
  • 156
  • 157
  • 158
  • 159
  • 160
  • 161
  • 162
  • 163
  • 164
  • 165
  • 166
  • 167
  • 168
  • 169
  • 170
  • 171
  • 172
  • 173
  • 174
  • 175
  • 176
  • 177
  • 178
  • 179
  • 180
  • 181
  • 182
  • 183
  • 184
  • 185
  • 186
  • 187
  • 188
  • 189
  • 190
  • 191
  • 192
  • 193
  • 194
  • 195
  • 196
  • 197
  • 198
  • 199
  • 200
  • 201
  • 202
  • 203
  • 204
  • 205
  • 206
  • 207
  • 208
  • 209
  • 210
  • 211
  • 212
  • 213
  • 214
  • 215
  • 216
  • 217
  • 218
  • 219
  • 220
  • 221

5. TF树管理

5.1 TF树监听器
#!/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监听器和查询”“”

<span class="token keyword">def</span> <span class="token function">__init__</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    rospy<span class="token punctuation">.</span>init_node<span class="token punctuation">(</span><span class="token string">'tf_listener'</span><span class="token punctuation">)</span>

    <span class="token comment"># 创建TF缓冲区和监听器</span>
    self<span class="token punctuation">.</span>tf_buffer <span class="token operator">=</span> tf2_ros<span class="token punctuation">.</span>Buffer<span class="token punctuation">(</span><span class="token punctuation">)</span>
    self<span class="token punctuation">.</span>tf_listener <span class="token operator">=</span> tf2_ros<span class="token punctuation">.</span>TransformListener<span class="token punctuation">(</span>self<span class="token punctuation">.</span>tf_buffer<span class="token punctuation">)</span>

    <span class="token comment"># 等待TF树建立</span>
    rospy<span class="token punctuation">.</span>sleep<span class="token punctuation">(</span><span class="token number">2.0</span><span class="token punctuation">)</span>

    <span class="token comment"># 演示各种TF查询</span>
    self<span class="token punctuation">.</span>demonstrate_tf_queries<span class="token punctuation">(</span><span class="token punctuation">)</span>

    <span class="token comment"># 设置定时器持续查询</span>
    self<span class="token punctuation">.</span>timer <span class="token operator">=</span> rospy<span class="token punctuation">.</span>Timer<span class="token punctuation">(</span>rospy<span class="token punctuation">.</span>Duration<span class="token punctuation">(</span><span class="token number">1.0</span><span class="token punctuation">)</span><span class="token punctuation">,</span> self<span class="token punctuation">.</span>timer_callback<span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">demonstrate_tf_queries</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""演示TF查询"""</span>

    <span class="token keyword">try</span><span class="token punctuation">:</span>
        <span class="token comment"># 1. 查询最新的变换</span>
        self<span class="token punctuation">.</span>query_latest_transform<span class="token punctuation">(</span><span class="token punctuation">)</span>

        <span class="token comment"># 2. 查询特定时间的变换</span>
        self<span class="token punctuation">.</span>query_transform_at_time<span class="token punctuation">(</span><span class="token punctuation">)</span>

        <span class="token comment"># 3. 变换点</span>
        self<span class="token punctuation">.</span>transform_point<span class="token punctuation">(</span><span class="token punctuation">)</span>

        <span class="token comment"># 4. 变换位姿</span>
        self<span class="token punctuation">.</span>transform_pose<span class="token punctuation">(</span><span class="token punctuation">)</span>

        <span class="token comment"># 5. 查询所有坐标系</span>
        self<span class="token punctuation">.</span>list_all_frames<span class="token punctuation">(</span><span class="token punctuation">)</span>

        <span class="token comment"># 6. 检查变换是否存在</span>
        self<span class="token punctuation">.</span>check_transform_existence<span class="token punctuation">(</span><span class="token punctuation">)</span>

    <span class="token keyword">except</span> <span class="token punctuation">(</span>tf2_ros<span class="token punctuation">.</span>LookupException<span class="token punctuation">,</span> tf2_ros<span class="token punctuation">.</span>ConnectivityException<span class="token punctuation">,</span>
            tf2_ros<span class="token punctuation">.</span>ExtrapolationException<span class="token punctuation">)</span> <span class="token keyword">as</span> e<span class="token punctuation">:</span>
        rospy<span class="token punctuation">.</span>logerr<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"TF Error: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>e<span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">query_latest_transform</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""查询最新的变换"""</span>
    <span class="token keyword">try</span><span class="token punctuation">:</span>
        <span class="token comment"># 获取base_link到laser_link的最新变换</span>
        transform <span class="token operator">=</span> self<span class="token punctuation">.</span>tf_buffer<span class="token punctuation">.</span>lookup_transform<span class="token punctuation">(</span>
            <span class="token string">'base_link'</span><span class="token punctuation">,</span> <span class="token string">'laser_link'</span><span class="token punctuation">,</span> rospy<span class="token punctuation">.</span>Time<span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span>

        rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string">"Latest transform from base_link to laser_link:"</span><span class="token punctuation">)</span>
        rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"  Translation: (</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, "</span></span>
                     <span class="token string-interpolation"><span class="token string">f"</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, "</span></span>
                     <span class="token string-interpolation"><span class="token string">f"</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>z<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">)"</span></span><span class="token punctuation">)</span>

        <span class="token comment"># 提取欧拉角</span>
        <span class="token keyword">import</span> tf<span class="token punctuation">.</span>transformations <span class="token keyword">as</span> tf_trans
        quaternion <span class="token operator">=</span> <span class="token punctuation">(</span>
            transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>x<span class="token punctuation">,</span>
            transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>y<span class="token punctuation">,</span>
            transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>z<span class="token punctuation">,</span>
            transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>rotation<span class="token punctuation">.</span>w
        <span class="token punctuation">)</span>
        euler <span class="token operator">=</span> tf_trans<span class="token punctuation">.</span>euler_from_quaternion<span class="token punctuation">(</span>quaternion<span class="token punctuation">)</span>
        rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"  Rotation (RPY): (</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>euler<span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>euler<span class="token punctuation">[</span><span class="token number">1</span><span class="token punctuation">]</span><span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>euler<span class="token punctuation">[</span><span class="token number">2</span><span class="token punctuation">]</span><span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">)"</span></span><span class="token punctuation">)</span>

    <span class="token keyword">except</span> tf2_ros<span class="token punctuation">.</span>TransformException <span class="token keyword">as</span> e<span class="token punctuation">:</span>
        rospy<span class="token punctuation">.</span>logwarn<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Could not get transform: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>e<span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">query_transform_at_time</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""查询特定时间的变换"""</span>
    <span class="token keyword">try</span><span class="token punctuation">:</span>
        <span class="token comment"># 查询1秒前的变换</span>
        past_time <span class="token operator">=</span> rospy<span class="token punctuation">.</span>Time<span class="token punctuation">.</span>now<span class="token punctuation">(</span><span class="token punctuation">)</span> <span class="token operator">-</span> rospy<span class="token punctuation">.</span>Duration<span class="token punctuation">(</span><span class="token number">1.0</span><span class="token punctuation">)</span>

        <span class="token keyword">if</span> self<span class="token punctuation">.</span>tf_buffer<span class="token punctuation">.</span>can_transform<span class="token punctuation">(</span><span class="token string">'odom'</span><span class="token punctuation">,</span> <span class="token string">'base_link'</span><span class="token punctuation">,</span> past_time<span class="token punctuation">)</span><span class="token punctuation">:</span>
            transform <span class="token operator">=</span> self<span class="token punctuation">.</span>tf_buffer<span class="token punctuation">.</span>lookup_transform<span class="token punctuation">(</span>
                <span class="token string">'odom'</span><span class="token punctuation">,</span> <span class="token string">'base_link'</span><span class="token punctuation">,</span> past_time<span class="token punctuation">)</span>

            rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Transform at </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>past_time<span class="token punctuation">.</span>to_sec<span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">:</span><span class="token format-spec">.2f</span><span class="token punctuation">}</span></span><span class="token string">:"</span></span><span class="token punctuation">)</span>
            rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"  Position: (</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, "</span></span>
                         <span class="token string-interpolation"><span class="token string">f"</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">)"</span></span><span class="token punctuation">)</span>

    <span class="token keyword">except</span> tf2_ros<span class="token punctuation">.</span>TransformException <span class="token keyword">as</span> e<span class="token punctuation">:</span>
        rospy<span class="token punctuation">.</span>logwarn<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Could not get past transform: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>e<span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">transform_point</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""变换点"""</span>
    <span class="token comment"># 创建一个在laser_link坐标系中的点</span>
    point_laser <span class="token operator">=</span> PointStamped<span class="token punctuation">(</span><span class="token punctuation">)</span>
    point_laser<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"laser_link"</span>
    point_laser<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> rospy<span class="token punctuation">.</span>Time<span class="token punctuation">(</span><span class="token punctuation">)</span>
    point_laser<span class="token punctuation">.</span>point<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">1.0</span>  <span class="token comment"># 激光雷达前方1米</span>
    point_laser<span class="token punctuation">.</span>point<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0.0</span>
    point_laser<span class="token punctuation">.</span>point<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">0.0</span>

    <span class="token keyword">try</span><span class="token punctuation">:</span>
        <span class="token comment"># 变换到base_link坐标系</span>
        point_base <span class="token operator">=</span> self<span class="token punctuation">.</span>tf_buffer<span class="token punctuation">.</span>transform<span class="token punctuation">(</span>point_laser<span class="token punctuation">,</span> <span class="token string">"base_link"</span><span class="token punctuation">)</span>

        rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string">"Point transformation:"</span><span class="token punctuation">)</span>
        rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"  In laser_link: (</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>point_laser<span class="token punctuation">.</span>point<span class="token punctuation">.</span>x<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, "</span></span>
                     <span class="token string-interpolation"><span class="token string">f"</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>point_laser<span class="token punctuation">.</span>point<span class="token punctuation">.</span>y<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>point_laser<span class="token punctuation">.</span>point<span class="token punctuation">.</span>z<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">)"</span></span><span class="token punctuation">)</span>
        rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"  In base_link: (</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>point_base<span class="token punctuation">.</span>point<span class="token punctuation">.</span>x<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, "</span></span>
                     <span class="token string-interpolation"><span class="token string">f"</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>point_base<span class="token punctuation">.</span>point<span class="token punctuation">.</span>y<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>point_base<span class="token punctuation">.</span>point<span class="token punctuation">.</span>z<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">)"</span></span><span class="token punctuation">)</span>

    <span class="token keyword">except</span> tf2_ros<span class="token punctuation">.</span>TransformException <span class="token keyword">as</span> e<span class="token punctuation">:</span>
        rospy<span class="token punctuation">.</span>logwarn<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Could not transform point: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>e<span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">transform_pose</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""变换位姿"""</span>
    <span class="token comment"># 创建一个位姿</span>
    pose_camera <span class="token operator">=</span> PoseStamped<span class="token punctuation">(</span><span class="token punctuation">)</span>
    pose_camera<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id <span class="token operator">=</span> <span class="token string">"camera_link"</span>
    pose_camera<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp <span class="token operator">=</span> rospy<span class="token punctuation">.</span>Time<span class="token punctuation">(</span><span class="token punctuation">)</span>

    <span class="token comment"># 相机坐标系中检测到的物体</span>
    pose_camera<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>x <span class="token operator">=</span> <span class="token number">2.0</span>
    pose_camera<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>y <span class="token operator">=</span> <span class="token number">0.5</span>
    pose_camera<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>z <span class="token operator">=</span> <span class="token number">0.0</span>
    pose_camera<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>orientation<span class="token punctuation">.</span>w <span class="token operator">=</span> <span class="token number">1.0</span>

    <span class="token keyword">try</span><span class="token punctuation">:</span>
        <span class="token comment"># 变换到map坐标系</span>
        pose_map <span class="token operator">=</span> self<span class="token punctuation">.</span>tf_buffer<span class="token punctuation">.</span>transform<span class="token punctuation">(</span>pose_camera<span class="token punctuation">,</span> <span class="token string">"map"</span><span class="token punctuation">,</span>
                                           timeout<span class="token operator">=</span>rospy<span class="token punctuation">.</span>Duration<span class="token punctuation">(</span><span class="token number">1.0</span><span class="token punctuation">)</span><span class="token punctuation">)</span>

        rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string">"Object pose in map frame:"</span><span class="token punctuation">)</span>
        rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"  Position: (</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>pose_map<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>x<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, "</span></span>
                     <span class="token string-interpolation"><span class="token string">f"</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>pose_map<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>y<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">, "</span></span>
                     <span class="token string-interpolation"><span class="token string">f"</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>pose_map<span class="token punctuation">.</span>pose<span class="token punctuation">.</span>position<span class="token punctuation">.</span>z<span class="token punctuation">:</span><span class="token format-spec">.3f</span><span class="token punctuation">}</span></span><span class="token string">)"</span></span><span class="token punctuation">)</span>

    <span class="token keyword">except</span> tf2_ros<span class="token punctuation">.</span>TransformException <span class="token keyword">as</span> e<span class="token punctuation">:</span>
        rospy<span class="token punctuation">.</span>logwarn<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Could not transform pose: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>e<span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">list_all_frames</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""列出所有坐标系"""</span>
    frames <span class="token operator">=</span> self<span class="token punctuation">.</span>tf_buffer<span class="token punctuation">.</span>all_frames_as_yaml<span class="token punctuation">(</span><span class="token punctuation">)</span>
    rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string">"All TF frames:"</span><span class="token punctuation">)</span>
    <span class="token comment"># 解析YAML输出</span>
    lines <span class="token operator">=</span> frames<span class="token punctuation">.</span>split<span class="token punctuation">(</span><span class="token string">'\n'</span><span class="token punctuation">)</span>
    frame_names <span class="token operator">=</span> <span class="token punctuation">[</span>line<span class="token punctuation">.</span>split<span class="token punctuation">(</span><span class="token string">':'</span><span class="token punctuation">)</span><span class="token punctuation">[</span><span class="token number">0</span><span class="token punctuation">]</span><span class="token punctuation">.</span>strip<span class="token punctuation">(</span><span class="token punctuation">)</span>
                  <span class="token keyword">for</span> line <span class="token keyword">in</span> lines <span class="token keyword">if</span> <span class="token string">':'</span> <span class="token keyword">in</span> line<span class="token punctuation">]</span>
    <span class="token keyword">for</span> frame <span class="token keyword">in</span> frame_names<span class="token punctuation">[</span><span class="token punctuation">:</span><span class="token number">10</span><span class="token punctuation">]</span><span class="token punctuation">:</span>  <span class="token comment"># 只显示前10个</span>
        rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"  - </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>frame<span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">check_transform_existence</span><span class="token punctuation">(</span>self<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""检查变换是否存在"""</span>
    frame_pairs <span class="token operator">=</span> <span class="token punctuation">[</span>
        <span class="token punctuation">(</span><span class="token string">'map'</span><span class="token punctuation">,</span> <span class="token string">'odom'</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token punctuation">(</span><span class="token string">'odom'</span><span class="token punctuation">,</span> <span class="token string">'base_link'</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token punctuation">(</span><span class="token string">'base_link'</span><span class="token punctuation">,</span> <span class="token string">'laser_link'</span><span class="token punctuation">)</span><span class="token punctuation">,</span>
        <span class="token punctuation">(</span><span class="token string">'base_link'</span><span class="token punctuation">,</span> <span class="token string">'non_existent_frame'</span><span class="token punctuation">)</span>
    <span class="token punctuation">]</span>

    <span class="token keyword">for</span> source<span class="token punctuation">,</span> target <span class="token keyword">in</span> frame_pairs<span class="token punctuation">:</span>
        <span class="token keyword">if</span> self<span class="token punctuation">.</span>tf_buffer<span class="token punctuation">.</span>can_transform<span class="token punctuation">(</span>target<span class="token punctuation">,</span> source<span class="token punctuation">,</span> rospy<span class="token punctuation">.</span>Time<span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span><span class="token punctuation">:</span>
            rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Transform </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>source<span class="token punctuation">}</span></span><span class="token string"> -&gt; </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>target<span class="token punctuation">}</span></span><span class="token string">: EXISTS"</span></span><span class="token punctuation">)</span>
        <span class="token keyword">else</span><span class="token punctuation">:</span>
            rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Transform </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>source<span class="token punctuation">}</span></span><span class="token string"> -&gt; </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>target<span class="token punctuation">}</span></span><span class="token string">: NOT FOUND"</span></span><span class="token punctuation">)</span>

<span class="token keyword">def</span> <span class="token function">timer_callback</span><span class="token punctuation">(</span>self<span class="token punctuation">,</span> event<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""定时查询回调"""</span>
    <span class="token keyword">try</span><span class="token punctuation">:</span>
        <span class="token comment"># 持续跟踪机器人位置</span>
        transform <span class="token operator">=</span> self<span class="token punctuation">.</span>tf_buffer<span class="token punctuation">.</span>lookup_transform<span class="token punctuation">(</span>
            <span class="token string">'map'</span><span class="token punctuation">,</span> <span class="token string">'base_link'</span><span class="token punctuation">,</span> rospy<span class="token punctuation">.</span>Time<span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">)</span>

        x <span class="token operator">=</span> transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>x
        y <span class="token operator">=</span> transform<span class="token punctuation">.</span>transform<span class="token punctuation">.</span>translation<span class="token punctuation">.</span>y

        rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Robot position in map: (</span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>x<span class="token punctuation">:</span><span class="token format-spec">.2f</span><span class="token punctuation">}</span></span><span class="token string">, </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>y<span class="token punctuation">:</span><span class="token format-spec">.2f</span><span class="token punctuation">}</span></span><span class="token string">)"</span></span><span class="token punctuation">)</span>

    <span class="token keyword">except</span> tf2_ros<span class="token punctuation">.</span>TransformException<span class="token punctuation">:</span>
        <span class="token keyword">pass</span>  <span class="token comment"># 忽略错误</span>

<span class="token keyword">def</span> <span class="token function">transform_point_cloud</span><span class="token punctuation">(</span>self<span class="token punctuation">,</span> cloud_msg<span class="token punctuation">,</span> target_frame<span class="token punctuation">)</span><span class="token punctuation">:</span>
    <span class="token triple-quoted-string string">"""变换点云"""</span>
    <span class="token keyword">try</span><span class="token punctuation">:</span>
        <span class="token comment"># 获取变换</span>
        transform <span class="token operator">=</span> self<span class="token punctuation">.</span>tf_buffer<span class="token punctuation">.</span>lookup_transform<span class="token punctuation">(</span>
            target_frame<span class="token punctuation">,</span>
            cloud_msg<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id<span class="token punctuation">,</span>
            cloud_msg<span class="token punctuation">.</span>header<span class="token punctuation">.</span>stamp<span class="token punctuation">,</span>
            rospy<span class="token punctuation">.</span>Duration<span class="token punctuation">(</span><span class="token number">1.0</span><span class="token punctuation">)</span><span class="token punctuation">)</span>

        <span class="token comment"># 变换点云（需要tf2_sensor_msgs）</span>
        <span class="token comment"># transformed_cloud = tf2_sensor_msgs.do_transform_cloud(cloud_msg, transform)</span>

        rospy<span class="token punctuation">.</span>loginfo<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Transformed point cloud from </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>cloud_msg<span class="token punctuation">.</span>header<span class="token punctuation">.</span>frame_id<span class="token punctuation">}</span></span><span class="token string"> to </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>target_frame<span class="token punctuation">}</span></span><span class="token string">"</span></span><span class="token punctuation">)</span>

    <span class="token keyword">except</span> tf2_ros<span class="token punctuation">.</span>TransformException <span class="token keyword">as</span> e<span class="token punctuation">:</span>
        rospy<span class="token punctuation">.</span>logwarn<span class="token punctuation">(</span><span class="token string-interpolation"><span class="token string">f"Could not transform point cloud: </span><span class="token interpolation"><span class="token punctuation">{<!-- --></span>e<span class="token punctuation">}</span></
FlyingAnt_
关注
Qt-无标题窗口
学而思
05-21 9081
        今天学习了下无标题窗口，于是来记录下我的学习过程首先建立一个Qt Widgets Application继承QWidget类创建完后直接运行就得到了如下效果那么，既然是无标题窗口就要去掉窗口栏啊，于是加上setWindowFlags(Qt::FramelessWindowHint | Qt::WindowMinimizeButtonHint);去掉标题栏，但是这样整个窗口就不能移动...
Delphi无标题窗体增强功能实现：让无标题窗体更具交互性
gitblog_06708的博客
04-12 867
Delphi无标题窗体增强功能实现：让无标题窗体更具交互性 去发现同类优质开源项目:https://gitcode.com/ 项目介绍 在软件开发领域，Delphi 语言因其高效的执行速度和清晰的语法结构，一直受到开发者的喜爱。然而，在使用无标题窗体（borderstyle=bsnone）时，开发者常常面临一个问题：用户无法通过常规方式来移动或调整窗体大小。本项目正是为了解决这一难题而诞生，它提供...
无标题栏窗口拖动的实现
ArduousBonze的专栏
05-15 3906
普通的Windows应用程序窗口都具有统一的窗口风格，它们一般都包括：用户区和非用户区两部分。非用户区包括标题条、窗口边框、最大化按钮、最小化按钮、关闭按钮和系统默认的快捷键及鼠标支持等。利用鼠标的左键通过拖动窗口的标题条可以在屏幕上任意移动整个窗口，当光标停止在窗口边框上时可以改变窗口的大小。在一些比较经典的软件中，有一类特殊的无标题条的窗口。这种窗口不需要改变窗口的大小，但必须具有窗口客户区域
无标题窗口的拖动
lianxiao1314的专栏
10-06 860
普通的Windows应用程序窗口都具有统一的窗口风格，它们一般都包括：用户区和非用户区两部分。非用户区包括标题条、窗口边框、最大化按钮、最小化按钮、关闭按钮和系统默认的快捷键及鼠标支持等。利用鼠标的左键通过拖动窗口的标题条可以在屏幕上任意移动整个窗口，当光标停止在窗口边框上时可以改变窗口的大小。在一些比较经典的软件中，有一类特殊的无标题条的窗口。这种窗口不需要改变窗口的大小，但必须具有窗口客户区域
C#拖动无标题栏窗体
Windows Mobile 开发历险
12-25 1867
很长时间没有写博客了，今天又解决并测试通过了一个Delphi很简单的问题——无标题栏窗体的拖动 。其实是个很简单的问题，但是还是想写下来，一方面加深自己的记忆，一方面与朋友们分享！DeviceAnywhere在Ioicn发布了，反响似乎一般，我想是因为.net FrameWork环境的问题吧，希望Microsoft再出sp的时候把.net FrameWorkx.x集成到操作系统里吧，这样大家都方便
202X年度工作成果展示.pptx
最新发布
06-23
202X年度工作成果展示.pptx
【Python + 半导体】生产批次全链路追溯代码
06-23
本资源提供一套完整的半导体生产批次全链路追溯Python代码，通过Pandas数据关联分析技术，实现从批次号出发，追踪产品生产的全生命周期信息。
stm32单片机项目资料课程设计文档C语言程序代码原理图电路PCB实例ATMEGA848-S52-贴片DXP资料
06-23
stm32单片机项目资料课程设计文档C语言程序代码原理图电路PCB实例ATMEGA848-S52-贴片DXP资料
【创新未发表】离网运行、储能配置与并网经济性比较研究（Matlab代码、Python、数据、word论文）
06-23
内容概要：本资源围绕“离网运行、储能配置与并网经济性比较研究”展开，是一份尚未公开发表的创新性研究成果，包含完整的Matlab与Python代码、仿真数据及Word格式论文。研究通过建立微电网系统模型，对比分析离网模式下的储能配置方案与并网模式下的经济性表现，综合考虑可再生能源出力波动、负荷需求特性及运行成本等因素，利用优化算法进行多场景仿真与经济评估，旨在为分布式能源系统的规划与运行决策提供量化依据和技术支持。; 适合人群：具备电力系统、新能源或优化算法基础，从事相关领域科研工作的研究生、高校教师及工程技术人员。; 使用场景及目标：①用于微电网系统在不同运行模式（离网/并网）下的技术经济性对比研究；②学习储能容量优化配置方法与经济性分析建模流程；③复现论文结果或作为课题研究的参考模板。; 阅读建议：建议结合提供的代码、数据与论文文档同步阅读，重点关注模型构建逻辑、目标函数设计与算法实现细节，可通过修改参数进行多场景仿真以加深理解。
FS5175.6NTC保护使用说明.pdf
06-23
FS5175.6NTC保护使用说明
ASC4056单节锂电池1A线性充电IC.pdf
06-23
ASC4056单节锂电池1A线性充电IC
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值