ROS2---常用工具
TF：机器人坐标系管理神器
坐标系是机器人学中的基础概念，在完整的机器人系统中往往涉及多个坐标系，如何有效管理这些坐标系之间的位置关系？
ROS为此提供了专门的管理工具——TF坐标系系统。
机器人中的坐标系
比如在机械臂形态的机器人中，机器人安装的位置叫做基坐标系Base Frame，机器人安装位置在外部环境下的参考系叫做世界坐标系World Frame，机器人末端夹爪的位置叫做工具坐标系，外部被操作物体的位置叫做工件坐标系，在机械臂抓取外部物体的过程中，这些坐标系之间的关系也在跟随变化。
在移动机器人系统中，坐标系一样至关重要，比如一个移动机器人的中心点是基坐标系Base Link，雷达所在的位置叫做雷达坐标系laser link，机器人要移动，里程计会累积位置，这个位置的参考系叫做里程计坐标系odom，里程计又会有累积误差和漂移，绝对位置的参考系叫做地图坐标系map。
一层一层坐标系之间关系复杂，有一些是相对固定的，也有一些是不断变化的，看似简单的坐标系也在空间范围内变得复杂，良好的坐标系管理系统就显得格外重要。
关于坐标系变换关系的基本理论，在每一本机器人学的教材中都会有讲解，可以分解为平移和旋转两个部分，通过一个四乘四的矩阵进行描述，在空间中画出坐标系，那两者之间的变换关系，其实就是向量的数学描述。
ROS中TF功能的底层原理，就是对这些数学变换进行了封装，详细的理论知识大家可以参考机器人学的教材，我们主要讲解TF坐标管理系统的使用方法。
TF命令行操作
小海龟跟随例程
这个示例需要我们先安装相应的功能包，然后就可以通过一个launch文件启动，之后我们可以控制其中的一只小海龟，另外一只小海龟会自动跟随运动。
$ sudo apt install ros-humble-turtle-tf2-py ros-humble-tf2-tools
$ sudo pip3 install transforms3d
具体运行效果
$ ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py
$ ros2 run turtlesim turtle_teleop_key
查看TF树
在当前运行的两只海龟中，有哪些坐标系呢，我们可以通过这个小工具来做查看。
$ ros2 run tf2_tools view_frames
默认在当前终端路径下生成了一个frames.pdf文件，打开之后，就可以看到系统中各个坐标系的关系了。
这是 ROS2 小海龟 tf2 演示的坐标系树，view_frames命令把 TF 变换关系导出成的图。
1.坐标系树结构 world 是总世界坐标系（相当于地图原点）
world → turtle1：世界坐标系到海龟 1 的坐标变换world → turtle2：世界坐标系到海龟 2 的坐标变换
两个海龟都是直接挂在
world下面，turtle1 和 turtle2 之间没有直接连线。
2.每根连线上参数通俗解读
Broadcaster: default_authority：谁在发布这个坐标变换，这里是 demo 程序Average rate:62：大概62Hz 高频发布坐标，刷新很快Buffer length:5.038：TF 缓存保存5 秒历史变换数据，tf2 默认缓存 5 秒Most recent transform：最新一帧坐标的时间戳Oldest transform：缓存里存的最老的那一帧时间- 关键知识点
虽然图上没有 turtle1 ↔ turtle2 的连线，但 tf2 可以靠
world做中间桥梁，计算出海龟 2 相对于海龟 1 的位置。 公式逻辑：turtle2相对于turtle1 = world→turtle1 的逆变换 × world→turtle2
对应 demo 现象
小海龟 demo 里，turtle2 会自动追着 turtle1 跑，就是 TF 监听节点做了上面这个坐标换算，拿到 turtle2 相对于 turtle1 的位置，控制运动。
查询坐标变换信息
只看到坐标系的结构还不行，如果我们想要知道某两个坐标系之间的具体关系，可以通过tf2_echo这个工具查看：
$ ros2 run tf2_ros tf2_echo turtle2 turtle1
运行成功后，终端中就会循环打印坐标系的变换数值了，由平移和旋转两个部分组成，还有旋转矩阵。
坐标系可视化
用可视化软件来做显示：
$ ros2 run rviz2 rviz2 -d $(ros2 pkg prefix --share turtle_tf2_py)/rviz/turtle_rviz.rviz
静态TF广播
前面我们提到，TF 的核心作用就是对坐标系进行管理，那不妨现在就动手管理一个试试？
在坐标变换中，最简单的场景莫过于相对位置保持不变的情况。比如你家的房子，只要不拆除，它的坐标基本就不会发生变化。
这种场景在机器人系统中同样十分常见，例如激光雷达与机器人底盘之间的位置关系，一旦安装完成，通常就不会再发生变动。
在 TF 中，这种情况被称为静态TF变换。接下来，我们就来看看如何在程序中实现它。
运行效果
启动终端，运行如下命令：
$ ros2 run learning_tf static_tf_broadcaster
$ ros2 run tf2_tools view_frames 可以看到当前系统中存在两个坐标系，一个是world，一个是house，两者之间的相对位置不会发生改变，通过一个静态的TF对象进行维护。
代码解析
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
from geometry_msgs.msg import TransformStamped # 坐标变换消息
import tf_transformations # TF坐标变换库
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster # TF静态坐标系广播器类
class StaticTFBroadcaster(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.tf_broadcaster = StaticTransformBroadcaster(self) # 创建一个TF广播器对象
static_transformStamped = TransformStamped() # 创建一个坐标变换的消息对象
static_transformStamped.header.stamp = self.get_clock().now().to_msg() # 设置坐标变换消息的时间戳
static_transformStamped.header.frame_id = 'world' # 设置一个坐标变换的源坐标系
static_transformStamped.child_frame_id = 'house' # 设置一个坐标变换的目标坐标系
static_transformStamped.transform.translation.x = 10.0 # 设置坐标变换中的X、Y、Z向的平移
static_transformStamped.transform.translation.y = 5.0
static_transformStamped.transform.translation.z = 0.0
quat = tf_transformations.quaternion_from_euler(0.0, 0.0, 0.0) # 将欧拉角转换为四元数（roll, pitch, yaw）
static_transformStamped.transform.rotation.x = quat[0] # 设置坐标变换中的X、Y、Z向的旋转（四元数）
static_transformStamped.transform.rotation.y = quat[1]
static_transformStamped.transform.rotation.z = quat[2]
static_transformStamped.transform.rotation.w = quat[3]
self.tf_broadcaster.sendTransform(static_transformStamped) # 广播静态坐标变换，广播后两个坐标系的位置关系保持不变
def main(args=None):
rclpy.init(args=args) # ROS2 Python接口初始化
node = StaticTFBroadcaster("static_tf_broadcaster") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown()类比 Java：ROS2 节点 ≈ Java 里的一个后台服务
Service；TF 广播器≈一个发布者 Publisher；TransformStamped就是 Java 的 DTO 数据传输对象。 这段代码功能：发布一组永远不变的坐标系：world(世界地图)→house(房子)，房子固定在世界坐标 x=10,y=5，姿态不旋转，属于静态 TF（StaticTF），只发一次，之后关系永久生效，不需要循环反复发。
整体类结构对比 Java
class StaticTFBroadcaster(Node):
等价 Java 伪代码：
// 继承ROS2框架提供的Node父类，就像Spring组件继承ApplicationListener
public class StaticTFBroadcaster extends Node {
private StaticTransformBroadcaster tf_broadcaster; // 成员变量：TF静态广播器
public StaticTFBroadcaster(String name){
super(name); // 调用父类Node构造，注册节点到ROS2框架
// 业务初始化
}
}
Node：ROS2 所有节点的父类，类比 Java Spring 的@Component，拿到节点就拥有日志、时钟、发布、订阅能力。StaticTransformBroadcaster：静态 TF 广播器，专门发固定不变坐标系；与之对应的TransformBroadcaster是动态 TF（海龟位置会变，要循环不停发）。
逐段拆解（Java 视角）
1. import 导入 = Java import 导包
import rclpy # ROS2核心运行时，≈ ros2 sdk jar包
from rclpy.node import Node # 节点基类
from geometry_msgs.msg import TransformStamped # DTO消息实体，类比Java POJO/DTO，存坐标变换
import tf_transformations # 工具库：欧拉角 <=> 四元数转换工具类
from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster
TransformStamped类比 Java 的 DTO，内部字段：
- header：时间戳、源坐标系
frame_id- child_frame_id：子坐标系名字
- transform：平移 (x,y,z) + 旋转四元数 (x,y,z,w)
2. 构造函数 __init__ 相当于 Java 构造方法
def __init__(self, name):
super().__init__(name)
self.tf_broadcaster = StaticTransformBroadcaster(self)
super().__init__(name)：父类 Node 初始化，向 ROS2 注册节点名字。StaticTransformBroadcaster(self)：把当前节点this传给广播器，广播器依附当前节点，用节点的时钟、资源。Java 理解：this作为依赖注入传给工具对象。
3. 组装 DTO TransformStamped（重点）
static_transformStamped = TransformStamped()
static_transformStamped.header.stamp = self.get_clock().now().to_msg()
static_transformStamped.header.frame_id = 'world'
static_transformStamped.child_frame_id = 'house'
static_transformStamped.transform.translation.x = 10.0
static_transformStamped.transform.translation.y = 5.0
static_transformStamped.transform.translation.z = 0.0
quat = tf_transformations.quaternion_from_euler(0.0, 0.0, 0.0)
static_transformStamped.transform.rotation.x = quat[0]
static_transformStamped.transform.rotation.y = quat[1]
static_transformStamped.transform.rotation.z = quat[2]
static_transformStamped.transform.rotation.w = quat[3]
Java 伪代码：
TransformStamped msg = new TransformStamped();
msg.header.stamp = node.getClock().now().toMsg(); // 打上当前时间戳
msg.header.frame_id = "world"; // 父坐标系：世界
msg.child_frame_id = "house"; // 子坐标系：房子
msg.transform.translation.x =10.0;
msg.transform.translation.y =5.0;
msg.transform.translation.z =0.0;
// 欧拉角(roll,pitch,yaw)转四元数工具函数
double[] quat = TfTransformations.quaternionFromEuler(0,0,0);
msg.transform.rotation.x = quat[0];
msg.transform.rotation.y = quat[1];
msg.transform.rotation.z = quat[2];
msg.transform.rotation.w = quat[3];
概念大白话：
world父坐标系，house子坐标系：house 这个房子，在 world 地图上，X 走 10 米，Y 走 5 米，没有旋转。 四元数：ROS 不用欧拉角存旋转，防止万向锁，工具类把我们易懂的 roll/pitch/yaw 转成四元数。
4. sendTransform 发送静态 TF
self.tf_broadcaster.sendTransform(static_transformStamped)
静态 TF 关键点：只调用一次 sendTransform，不需要定时器循环。 Java 类比：调用 publisher.publish (msg)。 StaticTF 内部逻辑：底层会自动循环重复发布，不需要业务代码写循环。 ✅动态 TF（海龟 turtle1/turtle2）：必须写定时器，每几十 ms 调用一次 sendTransform，位置实时更新。
5. main 函数，程序入口，类比 Java main
def main(args=None):
rclpy.init(args=args) // 初始化ROS2运行环境，类似SpringApplication.run()
node = StaticTFBroadcaster("static_tf_broadcaster")
rclpy.spin(node) // 节点自旋，阻塞等待，处理消息回调，相当于事件循环
node.destroy_node() // 销毁
rclpy.shutdown() // 关闭ROS2运行时
rclpy.spin(node)：非常关键！等价 Java 事件循环，监听节点事件，没有 spin，节点直接跑完退出。- 执行流程：初始化运行时 → 创建节点对象 → 进入 spin 死循环挂起程序 → Ctrl+C 打断，执行销毁、释放资源。
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'static_tf_broadcaster = learning_tf.static_tf_broadcaster:main',
],
},TF监听
启动一个终端，运行如下节点，就可以在终端中看到周期显示的坐标关系了。
$ ros2 run learning_tf tf_listener
代码解析
import rclpy # ROS2 Python接口库
from rclpy.node import Node # ROS2 节点类
import tf_transformations # TF坐标变换库
from tf2_ros import TransformException # TF左边变换的异常类
from tf2_ros.buffer import Buffer # 存储坐标变换信息的缓冲类
from tf2_ros.transform_listener import TransformListener # 监听坐标变换的监听器类
class TFListener(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.declare_parameter('source_frame', 'world') # 创建一个源坐标系名的参数
self.source_frame = self.get_parameter( # 优先使用外部设置的参数值，否则用默认值
'source_frame').get_parameter_value().string_value
self.declare_parameter('target_frame', 'house') # 创建一个目标坐标系名的参数
self.target_frame = self.get_parameter( # 优先使用外部设置的参数值，否则用默认值
'target_frame').get_parameter_value().string_value
self.tf_buffer = Buffer() # 创建保存坐标变换信息的缓冲区
self.tf_listener = TransformListener(self.tf_buffer, self) # 创建坐标变换的监听器
self.timer = self.create_timer(1.0, self.on_timer) # 创建一个固定周期的定时器，处理坐标信息
def on_timer(self):
try:
now = rclpy.time.Time() # 获取ROS系统的当前时间
trans = self.tf_buffer.lookup_transform( # 监听当前时刻源坐标系到目标坐标系的坐标变换
self.target_frame,
self.source_frame,
now)
except TransformException as ex: # 如果坐标变换获取失败，进入异常报告
self.get_logger().info(
f'Could not transform {self.target_frame} to {self.source_frame}: {ex}')
return
pos = trans.transform.translation # 获取位置信息
quat = trans.transform.rotation # 获取姿态信息（四元数）
euler = tf_transformations.euler_from_quaternion([quat.x, quat.y, quat.z, quat.w])
self.get_logger().info('Get %s --> %s transform: [%f, %f, %f] [%f, %f, %f]'
% (self.source_frame, self.target_frame, pos.x, pos.y, pos.z, euler[0], euler[1], euler[2]))
def main(args=None):
rclpy.init(args=args) # ROS2 Python接口初始化
node = TFListener("tf_listener") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口上一段是TF 广播（生产者），这段代码是 TF 监听消费者。 类比 Java：
Buffer= 内存缓存队列，存历史坐标变换数据；TransformListener= 事件监听器，订阅 TF 话题，收到数据自动塞到 Buffer 缓存；lookup_transform()= 缓存查询 API，不是网络阻塞调用，查本地内存；- ROS2 节点参数 ≈ SpringBoot
@Value配置参数； - Timer 定时器 ≈ Scheduled 定时任务；
- TransformException ≈ Java 受检异常 try‑catch 捕获。
功能：每秒查一次 source_frame → target_frame 的坐标变换，打印平移 + 欧拉角姿态。默认查询 world 到 house。
逐模块拆解
1. import 导入
python
运行
from tf2_ros import TransformException # TF查询异常，类比Java Exception
from tf2_ros.buffer import Buffer # TF内存缓冲区
from tf2_ros.transform_listener import TransformListener # TF监听器
Buffer：核心存储层，TF 所有历史变换全部存在这里，默认缓存 5 秒历史数据。
重点：网络接收 TF 消息这件事，是
TransformListener干的；业务查询，是Buffer.lookup_transform()。二者解耦。 架构分层：Listener(网络订阅层)→Buffer(内存存储层)→ 业务代码调用 Buffer 查询。
2. ROS 参数部分
python
运行
self.declare_parameter('source_frame', 'world')
self.source_frame = self.get_parameter('source_frame').get_parameter_value().string_value
类比 SpringBoot @Value("${source_frame:world}")
declare_parameter：向 ROS 框架注册参数，给默认值；- 运行时可以命令行或者 launch 文件覆盖：
bash
ros2 run xxx tf_listener --ros-args -p source_frame:=turtle1 -p target_frame:=turtle2
3. Buffer + TransformListener 初始化
python
运行
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
TransformListener(self.tf_buffer, self)： 第二个参数传入节点对象self，监听器复用节点的时钟、话题订阅器。 底层自动订阅两个话题：/tf（动态变换）、/tf_static（静态变换）。 收到网络上的 TF 消息，自动塞进 tf_buffer，业务代码不需要写任何订阅回调。
这是 TF2 设计精髓：你不用写 callback，数据自动进缓存；业务只需要定时去缓存查询。
4. Timer 定时器
python
运行
self.timer = self.create_timer(1.0, self.on_timer)
1 秒触发一次on_timer，类比 Java @Scheduled(fixedRate = 1000)。
为什么不用消息回调？因为业务需要主动查询某两个坐标系之间的关系，不是来一条 TF 消息处理一次。
5. on_timer 核心业务逻辑
def on_timer(self):
try:
now = rclpy.time.Time()
trans = self.tf_buffer.lookup_transform(
self.target_frame,
self.source_frame,
now)
except TransformException as ex:
self.get_logger().info(f'Could not transform ... {ex}')
return
lookup_transform(target_frame, source_frame, time)
语义：在 time 时刻，source_frame 坐标系，在 target_frame 坐标系下的位姿。 函数只访问本地内存 Buffer，不会发网络请求。
异常TransformException触发场景（对应 Java 捕获异常）：
- TF 数据还没广播出来，缓存为空；
- 查询的时间戳，已经超出 Buffer 缓存窗口（默认 5 秒）；
- TF 树断裂，两个坐标系没有连通路径。
拿到变换之后：
pos = trans.transform.translation
quat = trans.transform.rotation
# 四元数转欧拉角，方便人阅读
euler = tf_transformations.euler_from_quaternion([quat.x, quat.y, quat.z, quat.w])
self.get_logger().info(...)
ROS 内部传输旋转只用四元数，打印给人看的时候转成 roll/pitch/yaw 欧拉角。
6. main 入口
rclpy.init(args=args)
node = TFListener("tf_listener")
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
rclpy.spin(node)：驱动节点所有回调，Timer、Listener 内部订阅回调全部靠 spin 驱动。没有 spin，Listener 收不到任何 TF 数据。
完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'static_tf_broadcaster = learning_tf.static_tf_broadcaster:main',
'tf_listener = learning_tf.tf_listener:main',
],
},海龟跟随功能解析
启动终端后，通过如下命令启动例程：
$ ros2 launch learning_tf turtle_following_demo.launch.py
$ ros2 run turtlesim turtle_teleop_key
看到的效果和ROS自带的例程相同。
原理解析
在两只海龟的仿真器中，我们可以定义三个坐标系，比如仿真器的全局参考系叫做world，turtle1和turtle2坐标系在两只海龟的中心点，这样，turtle1和world坐标系的相对位置，就可以表示海龟1的位置，海龟2也同理。
要实现海龟2向海龟1运动，我们在两者中间做一个连线，再加一个箭头，怎么样，是不是有想起高中时学习的向量计算？我们说坐标变换的描述方法就是向量，所以在这个跟随例程中，用TF就可以很好的解决。
向量的长度表示距离，方向表示角度，有了距离和角度，我们随便设置一个时间，不就可以计算得到速度了么，然后就是速度话题的封装和发布，海龟2也就可以动起来了。
所以这个例程的核心就是通过坐标系实现向量的计算，两只海龟还会不断运动，这个向量也得按照某一个周期计算，这就得用上TF的动态广播与监听了。
我们一起看下代码该如何实现。
Launch文件解析
先来看下刚才运行的launch文件，里边启动了四个节点，分别是：
- 小海龟仿真器
- 海龟1的坐标系广播
- 海龟2的坐标系广播
- 海龟跟随控制
其中，两个坐标系的广播复用了turtle_tf_broadcaster节点，通过传入的参数名修改维护的坐标系名称。
def generate_launch_description():
return LaunchDescription([
Node(
package='turtlesim',
executable='turtlesim_node',
name='sim'
),
Node(
package='learning_tf',
executable='turtle_tf_broadcaster',
name='broadcaster1',
parameters=[
{'turtlename': 'turtle1'}
]
),
DeclareLaunchArgument(
'target_frame', default_value='turtle1',
description='Target frame name.'
),
Node(
package='learning_tf',
executable='turtle_tf_broadcaster',
name='broadcaster2',
parameters=[
{'turtlename': 'turtle2'}
]
),
Node(
package='learning_tf',
executable='turtle_following',
name='listener',
parameters=[
{'target_frame': LaunchConfiguration('target_frame')}
]
),
])1、LaunchDescription([ ... ])
里面放一长串要启动的东西，按顺序编排：节点、启动参数、环境变量。 类比：批量任务清单，launch 工具会把清单里面每一项挨个执行。
2、Node(...) 代表启动一个 ROS 节点
package='xxx'：是哪个功能包executable='xxx'：这个包里面可执行程序名字name='sim'：给运行起来的节点起别名，覆盖代码内部写的节点名parameters=[{key:value}]：给节点传参数。
第 1 个 Node：启动
turtlesim仿真窗口，画出海龟。
第 2 个 Node
broadcaster1： 运行turtle_tf_broadcaster程序，参数turtlename=turtle1。 功能：读取 turtle1 海龟的位置，不停广播 world → turtle1 的 TF 坐标变换。
第 3 个 Node
broadcaster2： 同样一份广播程序，传参turtlename=turtle2，广播world → turtle2。
✅重点：同一份可执行程序，可以启动多份节点实例，靠传参区分业务对象。就像同一个 Java 类 new 出来两个不同对象。
3、DeclareLaunchArgument
声明一个launch 启动参数，名字叫target_frame，默认值turtle1。 运行 launch 的时候可以外部修改这个值。
示例运行命令：
bash
# 使用默认：turtle2追turtle1
ros2 launch learning_tf turtle_tf2_demo.launch.py
# 手动传参，turtle2去追别的坐标系
ros2 launch learning_tf turtle_tf2_demo.launch.py target_frame:=xxx
4、LaunchConfiguration('target_frame')
把上面声明的启动参数，传递给listener节点的参数。
效果：listener 节点拿到
target_frame，就知道要跟踪哪个坐标系。 listener 内部逻辑：不停 lookup_transform 查询 turtle2 相对于 target_frame 的位置，发布速度指令，控制海龟追过去。
坐标系动态广播
海龟1和海龟2在world坐标系下的坐标变换，在turtle_tf_broadcaster节点中实现，除了海龟坐标系的名字不同之外，针对两个海龟的功能是一样的。
class TurtleTFBroadcaster(Node):
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.declare_parameter('turtlename', 'turtle') # 创建一个海龟名称的参数
self.turtlename = self.get_parameter( # 优先使用外部设置的参数值，否则用默认值
'turtlename').get_parameter_value().string_value
self.tf_broadcaster = TransformBroadcaster(self) # 创建一个TF坐标变换的广播对象并初始化
self.subscription = self.create_subscription( # 创建一个订阅者，订阅海龟的位置消息
Pose,
f'/{self.turtlename}/pose', # 使用参数中获取到的海龟名称
self.turtle_pose_callback, 1)
def turtle_pose_callback(self, msg): # 创建一个处理海龟位置消息的回调函数，将位置消息转变成坐标变换
transform = TransformStamped() # 创建一个坐标变换的消息对象
transform.header.stamp = self.get_clock().now().to_msg() # 设置坐标变换消息的时间戳
transform.header.frame_id = 'world' # 设置一个坐标变换的源坐标系
transform.child_frame_id = self.turtlename # 设置一个坐标变换的目标坐标系
transform.transform.translation.x = msg.x # 设置坐标变换中的X、Y、Z向的平移
transform.transform.translation.y = msg.y
transform.transform.translation.z = 0.0
q = tf_transformations.quaternion_from_euler(0, 0, msg.theta) # 将欧拉角转换为四元数（roll, pitch, yaw）
transform.transform.rotation.x = q[0] # 设置坐标变换中的X、Y、Z向的旋转（四元数）
transform.transform.rotation.y = q[1]
transform.transform.rotation.z = q[2]
transform.transform.rotation.w = q[3]
# Send the transformation
self.tf_broadcaster.sendTransform(transform) # 广播坐标变换，海龟位置变化后，将及时更新坐标变换信息
def main(args=None):
rclpy.init(args=args) # ROS2 Python接口初始化
node = TurtleTFBroadcaster("turtle_tf_broadcaster") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口完成代码的编写后需要设置功能包的编译选项，让系统知道Python程序的入口，打开功能包的setup.py文件，加入如下入口点的配置：
entry_points={
'console_scripts': [
'static_tf_broadcaster = learning_tf.static_tf_broadcaster:main',
'turtle_tf_broadcaster = learning_tf.turtle_tf_broadcaster:main',
'tf_listener = learning_tf.tf_listener:main',
],
},海龟跟随
def __init__(self, name):
super().__init__(name) # ROS2节点父类初始化
self.declare_parameter('source_frame', 'turtle1') # 创建一个源坐标系名的参数
self.source_frame = self.get_parameter( # 优先使用外部设置的参数值，否则用默认值
'source_frame').get_parameter_value().string_value
self.tf_buffer = Buffer() # 创建保存坐标变换信息的缓冲区
self.tf_listener = TransformListener(self.tf_buffer, self) # 创建坐标变换的监听器
self.spawner = self.create_client(Spawn, 'spawn') # 创建一个请求产生海龟的客户端
self.turtle_spawning_service_ready = False # 是否已经请求海龟生成服务的标志位
self.turtle_spawned = False # 海龟是否产生成功的标志位
self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1) # 创建跟随运动海龟的速度话题
self.timer = self.create_timer(1.0, self.on_timer) # 创建一个固定周期的定时器，控制跟随海龟的运动
def on_timer(self):
from_frame_rel = self.source_frame # 源坐标系
to_frame_rel = 'turtle2' # 目标坐标系
if self.turtle_spawning_service_ready: # 如果已经请求海龟生成服务
if self.turtle_spawned: # 如果跟随海龟已经生成
try:
now = rclpy.time.Time() # 获取ROS系统的当前时间
trans = self.tf_buffer.lookup_transform( # 监听当前时刻源坐标系到目标坐标系的坐标变换
to_frame_rel,
from_frame_rel,
now)
except TransformException as ex: # 如果坐标变换获取失败，进入异常报告
self.get_logger().info(
f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')
return
msg = Twist() # 创建速度控制消息
scale_rotation_rate = 1.0 # 根据海龟角度，计算角速度
msg.angular.z = scale_rotation_rate * math.atan2(
trans.transform.translation.y,
trans.transform.translation.x)
scale_forward_speed = 0.5 # 根据海龟距离，计算线速度
msg.linear.x = scale_forward_speed * math.sqrt(
trans.transform.translation.x ** 2 +
trans.transform.translation.y ** 2)
self.publisher.publish(msg) # 发布速度指令，海龟跟随运动
else: # 如果跟随海龟没有生成
if self.result.done(): # 查看海龟是否生成
self.get_logger().info(
f'Successfully spawned {self.result.result().name}')
self.turtle_spawned = True
else: # 依然没有生成跟随海龟
self.get_logger().info('Spawn is not finished')
else: # 如果没有请求海龟生成服务
if self.spawner.service_is_ready(): # 如果海龟生成服务器已经准备就绪
request = Spawn.Request() # 创建一个请求的数据
request.name = 'turtle2' # 设置请求数据的内容，包括海龟名、xy位置、姿态
request.x = float(4)
request.y = float(2)
request.theta = float(0)
self.result = self.spawner.call_async(request) # 发送服务请求
self.turtle_spawning_service_ready = True # 设置标志位，表示已经发送请求
else:
self.get_logger().info('Service is not ready') # 海龟生成服务器还没准备就绪的提示
def main(args=None):
rclpy.init(args=args) # ROS2 Python接口初始化
node = TurtleFollowing("turtle_following") # 创建ROS2节点对象并进行初始化
rclpy.spin(node) # 循环等待ROS2退出
node.destroy_node() # 销毁节点对象
rclpy.shutdown() # 关闭ROS2 Python接口
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
1
0- 0
已为社区贡献3条内容
所有评论(0)