牢骚：博主的习惯就是“能省就省”，所以出现过一次的全称，且后续上下文没有歧义项的时候，此名称第二次出现就是省略的简写模式，比如：Gazebo -> GZB，WorkSpace -> WS，Package -> PKG，Python -> Py，官方文档 -> 官文，官方网站 -> 官网，官方Git仓库 -> 官仓等等，但是仅仅局限于讲解/讲道理的文本区间，其余的比如具体代码等等则仍然采用真实名称，可放心CV。且除了及其基础的篇章或者使用方式差异较大的接口会特殊提及外，像Py:str(xxx.abc)/Cpp:xxx->abc.c_str()这种差异的代码一般只会使用最简单或者打字最少的语言单次描述。（原文链接：https://www.eeworld.com.cn/ar5qrb1）
牢骚：同义词会使用XX/XX这种形式，有助于阅读和理解和“偷换概念（bushi”，比如：张量/向量，松弛操作/短路径数值迭代/Relaxtion等等。
介绍：ROS2只是一个管理不同节点-Node及其数据传输的系统，面对基础的图像、音频、温湿度、蓝牙等等数据使用基础的消息传递机制如：话题-Topic即可实现。但是面对更复杂的场景，比如坐标系管理，代码新手使用基础的消息传递机制管理机器人的各项坐标就会变得比较困难。这时就可以使用官方推荐的TF2包来管理你的坐标系关系。
介绍：TF2会生成与维护一个坐标树，在这里所有的坐标系都是通过描述坐标之间的关系来构建的。比如："B坐标系位于A坐标系X=10处，C坐标系位于B坐标系Y=2处，D坐标系位于A坐标系X=-10处"这句话就描述/构建了一个坐标树。当其中某个坐标系位置发生改变，但是基于此坐标系的其余坐标系的相对位置未发生改变，则只需要在缓存更新此坐标系的相对位置即可，不需要更新每一个坐标系的位置。比如B位于A的X=10处，即使A坐标系改变，B仍然位于A的X=10处，无需改变缓存内B相对于A的位置关系/广播。
介绍：一个三维世界的刚体，使用位置/偏移-translation和角度-rotation就能描述其位姿(位置+姿态)-Pose，其相对于旋转轴的偏转角度/欧拉角(x y z)常用四元数来表示[w x y z]，工具库可以直接使用tf_transformations.quaternion_from_euler(x, y, z)来获得四元数，同理也有相应的函数将四元数转换为欧拉角。
抽象：可以将TF坐标树当作一个绘制地图的绘图员，TF广播器就是一个雷达兵，雷达兵随时上报位置，TF监听器就是一个战略家，随时问绘图员：在XX时间的时候雷达兵在哪？
graph TD
A[A坐标系] -->|X=10| B[B坐标系]
B -->|Y=2| C[C坐标系]
A -->|X=-10| D[D坐标系]
# 万一不支持Mermaid示意图，会看到这些注释
# A (0,0)
# |
# |-- B (10,0)
# | |
# | `-- C (10,2)
# |
# `-- D (-10,0)
之后的坐标例子默认使用此关系，直到“海龟跟随示例”小节，旋转矩阵、四元数、增广变换矩阵等将不赘述！其中旋转角度将会直接使用四元数[w x y z]表示：
q=[w,v]=[w,[x,y,z]]=w+xi+yj+zk
广播坐标使用
TF使用“广播”坐标来维护坐标树，每个坐标系拥有唯一的名称对应，节点若想获取自己某个时间点相对于某个坐标系的坐标可以使用坐标Buffer以及监听器进行坐标转化。思路类似于：
C自身为原点的坐标系为坐标系C
C要相对于坐标系B运动，C现在在B中的坐标偏移是(2, 3, 0)，旋转偏移为(1, 0, 0, 0)
那么可以将此坐标变化关系广播出去，这样子其他节点就可以定位到坐标系C相对于其他坐标系的位置注意：TF只管根据广播的坐标系关系维护坐标系树，以便于其他节点随时通过监听器获取位置和姿态
其中位置控制还得靠自己对应的功能模块！比如你即使广播B在A的X=233处，但是你实际上没有让海龟Move(X=233)，那么你就仅仅是谎报坐标了而已！一般情况下，携带里程计的机器人模块会有里程计回调自动更新里程计坐标系相对于世界/地图坐标系的偏移。
TF2的坐标广播与获取/监听：
from tf2_ros import TransformBroadcaster, StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
from tf2_ros import TransformException, Buffer, TransformListener
"""
TransformStamped: 位姿信息+坐标系信息(源坐标系名+目标坐标系名)+时间戳，表O在时间N有位姿X
TransformBroadcaster: 动态广播器
StaticTransformBroadcaster: 静态广播器
对于静态广播相对于动态广播的区别：
- 类似于Python的Tuple相对于List：
- 如果数据固定不更新为啥不使用开销小且符合语义且安全的元组？
- 且使用方式一致，参数一致，效果（若数据不会变化的情况下）一致
TransformListener：监听所有的坐标系变化，并存于一个Buffer
下面是描述位置关系的例子：
"""
classNode1(Node):
def__init__(self, node_name):
super.__init__(node_name)
# 定义TF广播器，此广播器与当前对象生命周期绑定
self.tf_broadcaster = TransformBroadcaster(self)
# 绘制初始/上述的ABCD坐标系位置关系
self.tf_broadcaster.sendTransform(B) # BCD是什么之后讲
self.tf_broadcaster.sendTransform(C) # 这里为了梳理逻辑就先省略了
self.tf_broadcaster.sendTransform(D) # 就当BCD是TransformStamped就好
# 此时TF的坐标树已经更新了对应的位置，可以使用监听器获取他们的坐标关系：
# 创建监听器，监听器负责将监听到的变换存到Buffer
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
# 获取 `当前时间` 下的相对于 `坐标系B原点` 的 `坐标系A` 所在的位姿关系
now = rclpy.time.Time()
try:
trans = self.tf_buffer.lookup_transform(
"B", "A", now
)
except TransformException as ex:
self.get_logger().info(f'无法获取变换: {ex}，可能是坐标系名称错误')
else:
self.get_logger().info(f'此时位姿关系为：{trans}')
# 现在可以设置一个位于坐标系A的(0 0 0)点，查看其在坐标系B中的位置：
from geometry_msgs.msg import PointStamped
from tf2_geometry_msgs import do_transform_point
point = PointStamped()
point.header.frame_id = 'A'
point.point.x = 0.0；point.point.y = 0.0；point.point.z = 0.0
self.get_logger().info(f'在坐标系B中，点的坐标为：{
do_transform_point(point, trans)
}')
---
# OK，主要内容讲完，那么来看看BCD都是什么，以B举例子：
B = TransformStamped()
B.header.stamp = self.get_clock().now().to_msg()
B.header.frame_id = 'A'# 定义B坐标系相对于A坐标系的偏移
B.child_frame_id = 'B'# 即：在A坐标系看，B位于：(10, 0, 0)-(0,0,0,0)
# 由于此时提到了坐标系A和B，那么TF就会记录此时含有两个坐标系A和B
B.transform.translation.x = 10.0
B.transform.translation.y = 0.0
B.transform.translation.z = 0.0
B = self.euler_to_quaternion(0, 0, pose.theta) # 将欧拉角转换为四元数
B.transform.rotation.x = q[0]
B.transform.rotation.y = q[1]
B.transform.rotation.z = q[2]
B.transform.rotation.w = q[3]
# 这里就是上面的广播坐标关系，在广播后，TF就会把两个坐标关系计入缓存/内存
self.tf_broadcaster.sendTransform(B)
# C和D同理，无非一个是CB关系，一个是DA关系，改改上面的数字即可……
海龟跟随示例
前情提要：你安装了ROS2，且通过键盘上下左右控制、代码创建生产者节点发布话题到'/<turtle_name>/cmd_vel'、命令行发布话题使用-rate控制频率、ros2 bag等等手段尝试了调教小海龟。所以上述内容不再赘述。
前情提要：你会创建工作空间、会编译以及启动自己的节点，会使用Launch批量启动节点。所以上述内容不再赘述。
前情提要：除了命令行的-rate外，你会使用Node::create_timer创建定时器来定时触发hello消息的发布。所以上述内容不再赘述。
前情提要：你会创建TF的广播器，当海龟1运动的时候，你能够创建一个节点订阅海龟的位置，从而使用TF广播器将海龟坐标广播/公布/更新到TF坐标树。所以上述内容不再赘述。
前情提要：你会创建TF的监听器，你可以使用定时器不断获取海龟1的位置。所以上述内容不再赘述。
所以现在，创建一个“海龟跟随示例”还缺什么？答：初中物理，计算距离和速度，当知道海龟1和海龟2当前的坐标，若想海龟2在N秒内到达海龟1的位置需要哪个方向需要多少速度，并作为话题发布到'/<turtle_name>/cmd_vel'！过于简单，所以上述内容不再赘述。
# 创建工作空间与包
mkdir -p heke_example/src
cd heke_example/src
# 创建包，安装对应依赖
ros2 pkg create --build-type ament_python turtle2node \
--dependencies rclpy geometry_msgs turtlesim tf2_ros tf2_geometry_msgs
# 新建并编辑turtle2node.py：
vim turtle2node/turtle2node.py
# 编辑之后，将此文件加入setup.py编译指导，后编译，后依次启动节点：
...
entry_points={
'console_scripts': [
'heke = turtle2node.turtle2node:main',
],
},
...
colcon build --packages-select turtle2node
至于源代码：
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, PointStamped, TransformStamped
from turtlesim.msg import Pose
from tf2_ros import TransformException, Buffer, TransformListener, TransformBroadcaster
from tf2_geometry_msgs import do_transform_point
from turtlesim.srv import Spawn
import math
classTurtleTfFollower(Node):
def__init__(self):
super().__init__('turtle_tf_follower')
# 创建TF2缓冲区和监听器
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
# 创建TF广播器，用于发布turtle1和turtle2的TF变换
self.tf_broadcaster = TransformBroadcaster(self)
# 创建发布器，用于发布控制turtle2速度的消息
self.publisher = self.create_publisher(Twist, '/turtle2/cmd_vel', 10)
# 订阅turtle1和turtle2的位姿
self.turtle1_pose_sub = self.create_subscription(Pose, '/turtle1/pose', self.turtle1_pose_callback, 10)
self.turtle2_pose_sub = self.create_subscription(Pose, '/turtle2/pose', self.turtle2_pose_callback, 10)
# 初始化turtle1和turtle2的位姿
self.turtle1_pose = Pose()
self.turtle2_pose = Pose()
# 创建定时器，定期执行跟随逻辑
self.timer = self.create_timer(0.1, self.follow_turtle)
# 生成第二个海龟
self.spawn_turtle2()
defspawn_turtle2(self):
"""生成第二个海龟（turtle2）"""
self.get_logger().info('正在生成turtle2...')
# 创建客户端，调用/spawn服务生成第二个海龟
self.spawn_client = self.create_client(Spawn, '/spawn')
whilenotself.spawn_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info('等待/spawn服务...')
# 设置生成海龟的请求参数
request = Spawn.Request()
request.x = 5.0# 初始x坐标
request.y = 5.0# 初始y坐标
request.theta = 0.0# 初始角度
request.name = 'turtle2'# 海龟名称
# 异步调用服务
future = self.spawn_client.call_async(request)
future.add_done_callback(self.spawn_callback)
defspawn_callback(self, future):
"""生成海龟的回调函数"""
try:
response = future.result()
if response.name == 'turtle2':
self.get_logger().info('成功生成turtle2')
exceptExceptionas e:
self.get_logger().error(f'生成turtle2失败: {e}')
defturtle1_pose_callback(self, msg):
"""turtle1位姿回调函数"""
self.turtle1_pose = msg
self.publish_tf('turtle1', self.turtle1_pose)
defturtle2_pose_callback(self, msg):
"""turtle2位姿回调函数"""
self.turtle2_pose = msg
self.publish_tf('turtle2', self.turtle2_pose)
defpublish_tf(self, turtle_name, pose):
"""发布turtle的TF变换"""
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = turtle_name
t.transform.translation.x = pose.x
t.transform.translation.y = pose.y
t.transform.translation.z = 0.0
# 将欧拉角转换为四元数
q = self.euler_to_quaternion(0, 0, pose.theta)
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]
# 发布TF变换
self.tf_broadcaster.sendTransform(t)
defeuler_to_quaternion(self, roll, pitch, yaw):
"""将欧拉角转换为四元数"""
cy = math.cos(yaw * 0.5)
sy = math.sin(yaw * 0.5)
cp = math.cos(pitch * 0.5)
sp = math.sin(pitch * 0.5)
cr = math.cos(roll * 0.5)
sr = math.sin(roll * 0.5)
q = [0] * 4
q[0] = sr * cp * cy - cr * sp * sy
q[1] = cr * sp * cy + sr * cp * sy
q[2] = cr * cp * sy - sr * sp * cy
q[3] = cr * cp * cy + sr * sp * sy
return q
deffollow_turtle(self):
"""实现turtle2跟随turtle1的逻辑"""
try:
# 获取从turtle2到turtle1的变换
transform = self.tf_buffer.lookup_transform(
'turtle2', # 目标坐标系
'turtle1', # 源坐标系
rclpy.time.Time(), # 获取最新的变换
timeout=rclpy.duration.Duration(seconds=1.0))
# 创建一个PointStamped消息，表示turtle1的位置
point = PointStamped()
point.header.frame_id = 'turtle1'
point.point.x = 0.0
point.point.y = 0.0
point.point.z = 0.0
# 将turtle1的位置转换到turtle2的坐标系中
transformed_point = do_transform_point(point, transform)
# 计算turtle2需要移动的速度
cmd_vel = Twist()
cmd_vel.linear.x = 0.5 * transformed_point.point.x # 线性速度
cmd_vel.angular.z = 4.0 * transformed_point.point.y # 角速度
# 发布速度命令
self.publisher.publish(cmd_vel)
except TransformException as ex:
self.get_logger().info(f'无法获取变换: {ex}')
defmain(args=None):
rclpy.init(args=args)
node = TurtleTfFollower()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
之后的启动节点（可自行编写Launch文件）：
# 海龟1：
ros2 run turtlesim turtlesim_node
# 海龟1的键盘控制
ros2 run turtlesim turtle_teleop_key
# 海龟2，别忘记启用ROS2工作空间/install/setup.bash的环境！
ros2 run turtle2node heke
效果展示：
查看坐标系关系：
TF2命令行工具：
ros2 run tf2_tools view_frames # 查看TF树，生成的.gv和.pdf自行找工具查看！
ros2 run tf2_ros tf2_echo <target_frame_id> <source_frame_id> # 查看坐标系关系
可视化坐标系
啊，受够了干巴巴的PDF坐标系指示，就没有直接一点纵览全局的坐标系展示方案？RViz：。
吐槽：本来想一次性讲完GRR(见开头src部分)三幻神的，果然还是放到下一篇比较好……
ros2 run rviz2 rviz2
弱弱提示：RViz可以点击左下角的Add按钮添加显示类型，添加两个坐标轴之后，设置引用的坐标系为对应的海龟即可。另外记得把全局坐标系设置为world！
之后在通过键盘控制第一只海归就能看到两个坐标轴追着跑了！当跑完还想下次一键展示，则可以保存配置为.rviz文件，下次启动的时候直接ros2 run rviz2 rviz2 -d xxx.rviz即可加载配置！