夜雨聆风目标： 学习如何向 tf2 添加额外的帧。
教程级别： 中级
时间： 15 分钟
背景
在之前的教程中，我们通过编写 tf2 广播器和 tf2 监听器重现了乌龟演示。本教程将教您如何向变换树添加额外的固定帧和动态帧。实际上，在 tf2 中添加帧与创建 tf2 广播器非常相似，但这个示例将向您展示 tf2 的一些额外功能。
对于许多与变换相关的任务，在局部帧中思考更容易。例如，在激光扫描仪中心的坐标系中推理激光扫描测量数据是最容易的。tf2 允许您为系统中的每个传感器、连杆或关节定义局部帧。当从一个帧变换到另一个帧时，tf2 将处理所有引入的隐藏中间帧变换。
tf2 树
tf2 构建了一个帧的树状结构，因此不允许帧结构中出现闭环。这意味着一个帧只有一个父帧，但可以有多个子帧。目前，我们的 tf2 树包含三个帧：world、turtle1 和 turtle2。两个乌龟帧是 world 帧的子帧。如果我们想向 tf2 添加一个新帧，三个现有帧中的一个需要是父帧，新帧将成为其子帧。
任务
1 编写固定帧广播器
在我们的乌龟示例中，我们将添加一个新帧 carrot1，它将是 turtle1 的子帧。此帧将作为第二只乌龟的目标。
首先创建源文件。转到我们在之前教程中创建的 learning_tf2_py 包。在 src/learning_tf2_py/learning_tf2_py 目录中，通过输入以下命令下载固定帧广播器代码：
Linux：
wget https://raw.githubusercontent.com/ros/geometry_tutorials/lyrical/turtle_tf2_py/turtle_tf2_py/fixed_frame_tf2_broadcaster.py现在打开名为 fixed_frame_tf2_broadcaster.py 的文件。
from geometry_msgs.msg import TransformStamped
import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
class FixedFrameBroadcaster(Node):
def __init__(self):
super().__init__('fixed_frame_tf2_broadcaster')
self.tf_broadcaster = TransformBroadcaster(self)
self.timer = self.create_timer(0.1, self.broadcast_timer_callback)
def broadcast_timer_callback(self):
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'turtle1'
t.child_frame_id = 'carrot1'
t.transform.translation.x = 0.0
t.transform.translation.y = 2.0
t.transform.translation.z = 0.0
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 0.0
t.transform.rotation.w = 1.0
self.tf_broadcaster.sendTransform(t)
def main():
try:
with rclpy.init():
node = FixedFrameBroadcaster()
rclpy.spin(node)
except (KeyboardInterrupt, ExternalShutdownException):
pass这段代码与 tf2 广播器教程示例非常相似，唯一的区别是这里的变换不随时间变化。
1.1 检查代码
让我们看一下这段代码中的关键行。这里我们创建一个新变换，从父帧 turtle1 到新的子帧 carrot1。carrot1 帧在 turtle1 帧的 y 轴方向偏移 2 米。
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'turtle1'
t.child_frame_id = 'carrot1'
t.transform.translation.x = 0.0
t.transform.translation.y = 2.0
t.transform.translation.z = 0.01.2 添加入口点
为了允许 ros2 run 命令运行您的节点，您必须将入口点添加到位于 src/learning_tf2_py 目录中的 setup.py。
在 'console_scripts': 括号之间添加以下行：
'fixed_frame_tf2_broadcaster = learning_tf2_py.fixed_frame_tf2_broadcaster:main',1.3 编写启动文件
现在让我们为这个示例创建一个启动文件。使用文本编辑器在 src/learning_tf2_py/launch 目录中创建一个名为 turtle_tf2_fixed_frame_demo_launch 的新文件（扩展名为 .py、.xml 或 .yaml），检查上一个教程启动文件的类型（.py、.xml 或 .yaml），并添加以下行：
Python 启动文件：
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
return LaunchDescription([
IncludeLaunchDescription(
PathJoinSubstitution([
FindPackageShare('learning_tf2_py'), 'launch', 'turtle_tf2_demo_launch.py'])
),
Node(
package='learning_tf2_py',
executable='fixed_frame_tf2_broadcaster',
name='fixed_broadcaster',
),
])此启动文件导入所需的包，然后创建一个 demo_nodes 变量，该变量将存储我们在上一个教程启动文件中创建的节点。
代码的最后一部分将使用我们的 fixed_frame_tf2_broadcaster 节点将固定的 carrot1 帧添加到 turtlesim 世界中。
Node(
package='learning_tf2_py',
executable='fixed_frame_tf2_broadcaster',
name='fixed_broadcaster',
),1.4 构建
在工作空间根目录运行 rosdep 来检查缺少的依赖项。
rosdep install -i --from-path src --rosdistro lyrical -y仍在工作空间根目录中，构建您的包：
colcon build --packages-select learning_tf2_py打开一个新终端，导航到工作空间根目录，并加载设置文件：
. install/setup.bash1.5 运行
现在您可以启动乌龟广播器演示：
ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo_launch.py您应该注意到新的 carrot1 帧出现在变换树中。
如果您驾驶第一只乌龟移动，您应该注意到行为与上一个教程相比没有变化，即使我们添加了一个新帧。这是因为添加额外帧不会影响其他帧，我们的监听器仍然使用之前定义的帧。
因此，如果我们希望第二只乌龟跟随胡萝卜而不是第一只乌龟，我们需要更改 target_frame 的值。这可以通过两种方式完成。一种方式是从控制台直接向启动文件传递 target_frame 参数：
ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo_launch.py target_frame:=carrot1第二种方式是更新启动文件。为此，打开 turtle_tf2_fixed_frame_demo_launch.py 文件，并通过 launch_arguments 参数添加 'target_frame': 'carrot1' 参数。
def generate_launch_description():
demo_nodes = IncludeLaunchDescription(
...,
launch_arguments={'target_frame': 'carrot1'}.items(),
)现在重新构建包，重新启动 turtle_tf2_fixed_frame_demo_launch.py，您将看到第二只乌龟跟随胡萝卜而不是第一只乌龟！
2 编写动态帧广播器
我们在本教程中发布的额外帧是一个相对于父帧不随时间变化的固定帧。但是，如果您想发布一个移动帧，您可以编写广播器使帧随时间变化。让我们改变 carrot1 帧，使其相对于 turtle1 帧随时间变化。转到我们在之前教程中创建的 learning_tf2_py 包。在 src/learning_tf2_py/learning_tf2_py 目录中，通过输入以下命令下载动态帧广播器代码：
Linux：
wget https://raw.githubusercontent.com/ros/geometry_tutorials/lyrical/turtle_tf2_py/turtle_tf2_py/dynamic_frame_tf2_broadcaster.py现在打开名为 dynamic_frame_tf2_broadcaster.py 的文件：
import math
from geometry_msgs.msg import TransformStamped
import rclpy
from rclpy.executors import ExternalShutdownException
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
class DynamicFrameBroadcaster(Node):
def __init__(self):
super().__init__('dynamic_frame_tf2_broadcaster')
self.tf_broadcaster = TransformBroadcaster(self)
self.timer = self.create_timer(0.1, self.broadcast_timer_callback)
def broadcast_timer_callback(self):
seconds, _ = self.get_clock().now().seconds_nanoseconds()
x = seconds * math.pi
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'turtle1'
t.child_frame_id = 'carrot1'
t.transform.translation.x = 10 * math.sin(x)
t.transform.translation.y = 10 * math.cos(x)
t.transform.translation.z = 0.0
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = 0.0
t.transform.rotation.w = 1.0
self.tf_broadcaster.sendTransform(t)
def main():
try:
with rclpy.init():
node = DynamicFrameBroadcaster()
rclpy.spin(node)
except (KeyboardInterrupt, ExternalShutdownException):
pass2.1 检查代码
我们不是固定定义 x 和 y 偏移量，而是使用当前时间上的 sin() 和 cos() 函数，使得 carrot1 的偏移量不断变化。
seconds, _ = self.get_clock().now().seconds_nanoseconds()
x = seconds * math.pi
...
t.transform.translation.x = 10 * math.sin(x)
t.transform.translation.y = 10 * math.cos(x)2.2 添加入口点
为了允许 ros2 run 命令运行您的节点，您必须将入口点添加到位于 src/learning_tf2_py 目录中的 setup.py。
在 'console_scripts': 括号之间添加以下行：
'dynamic_frame_tf2_broadcaster = learning_tf2_py.dynamic_frame_tf2_broadcaster:main',2.3 编写启动文件
要测试此代码，请在 src/learning_tf2_py/launch 目录中创建一个名为 turtle_tf2_dynamic_frame_demo_launch 的新启动文件（扩展名为 .py、.xml 或 .yaml），检查上一个教程启动文件的类型（.py、.xml 或 .yaml），并在以下代码中更新它：
Python 启动文件：
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
return LaunchDescription([
IncludeLaunchDescription(
PathJoinSubstitution([
FindPackageShare('learning_tf2_py'), 'launch', 'turtle_tf2_demo_launch.py']),
launch_arguments={'target_frame': 'carrot1'}.items(),
),
Node(
package='learning_tf2_py',
executable='dynamic_frame_tf2_broadcaster',
name='dynamic_broadcaster',
),
])2.4 构建
在工作空间根目录运行 rosdep 来检查缺少的依赖项。
rosdep install -i --from-path src --rosdistro lyrical -y仍在工作空间根目录中，构建您的包：
colcon build --packages-select learning_tf2_py打开一个新终端，导航到工作空间根目录，并加载设置文件：
. install/setup.bash2.5 运行
现在您可以启动动态帧演示：
ros2 launch learning_tf2_py turtle_tf2_dynamic_frame_demo_launch.py您应该会看到第二只乌龟跟随不断变化的胡萝卜位置。
总结
在本教程中，您了解了 tf2 变换树、其结构和特性。您还了解到在局部帧中思考是最容易的，并学习了为该局部帧添加额外的固定帧和动态帧。
