目标:学习如何向 tf2 添加额外的框架。
教程级别:中级
时间:15 分钟
目录
背景
tf2 树
任务
1 编写固定帧广播器
1.1 检查代码
1.2 添加入口点
1.3 编写启动文件
1.4 构建
1.5 运行
2 编写动态帧广播器
2.1 检查代码
2.2 添加入口点
2.3 编写启动文件
2.4 构建
2.5 运行
摘要
背景
在之前的教程中,我们通过编写 tf2 广播器和 tf2 监听器重新创建了乌龟演示。本教程将教您如何向变换树添加额外的固定和动态帧。事实上,在 tf2 中添加帧与创建 tf2 广播器非常相似,但本示例将向您展示 tf2 的一些附加功能。
对于许多与变换相关的任务,在局部坐标系中思考更容易。例如,最容易在激光扫描仪中心的坐标系中推理激光扫描测量值。tf2 允许您为系统中的每个传感器、链接或关节定义一个局部坐标系。当从一个坐标系变换到另一个坐标系时,tf2 将处理所有引入的隐藏中间坐标系变换。
任务
1 编写固定帧广播器
在我们的乌龟示例中,我们将添加一个新的框架 carrot1
,它将是 turtle1
的子框架。这个框架将作为第二只乌龟的目标。
首先创建源文件。进入我们在前面的教程中创建的 learning_tf2_py
包。在 src/learning_tf2_py/learning_tf2_py
目录中,通过输入以下命令下载固定帧广播代码:
cxy@ubuntu2404-cxy:~$ cd ~/ros2_ws/src/learning_tf2_py/learning_tf2_py
cxy@ubuntu2404-cxy:~/ros2_ws/src/learning_tf2_py/learning_tf2_py$ wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/fixed_frame_tf2_broadcaster.py
现在打开名为 fixed_frame_tf2_broadcaster.py
的文件。
from geometry_msgs.msg import TransformStamped # 从 geometry_msgs.msg 导入 TransformStamped 消息类型
import rclpy # 导入 rclpy 库
from rclpy.node import Node # 从 rclpy.node 导入 Node 类
from tf2_ros import TransformBroadcaster # 从 tf2_ros 导入 TransformBroadcaster 类
class FixedFrameBroadcaster(Node): # 定义一个名为 FixedFrameBroadcaster 的类,继承自 Node 类
def __init__(self): # 初始化方法
super().__init__('fixed_frame_tf2_broadcaster') # 调用父类的初始化方法,并设置节点名称为 'fixed_frame_tf2_broadcaster'
self.tf_broadcaster = TransformBroadcaster(self) # 创建一个 TransformBroadcaster 对象
self.timer = self.create_timer(0.1, self.broadcast_timer_callback) # 创建一个定时器,每 0.1 秒调用一次 broadcast_timer_callback 方法
def broadcast_timer_callback(self): # 定义定时器回调方法
t = TransformStamped() # 创建一个 TransformStamped 对象
t.header.stamp = self.get_clock().now().to_msg() # 设置时间戳为当前时间
t.header.frame_id = 'turtle1' # 设置父坐标系 ID 为 'turtle1'
t.child_frame_id = 'carrot1' # 设置子坐标系 ID 为 'carrot1'
t.transform.translation.x = 0.0 # 设置平移变换的 x 坐标为 0.0
t.transform.translation.y = 2.0 # 设置平移变换的 y 坐标为 2.0
t.transform.translation.z = 0.0 # 设置平移变换的 z 坐标为 0.0
t.transform.rotation.x = 0.0 # 设置旋转变换的 x 分量为 0.0
t.transform.rotation.y = 0.0 # 设置旋转变换的 y 分量为 0.0
t.transform.rotation.z = 0.0 # 设置旋转变换的 z 分量为 0.0
t.transform.rotation.w = 1.0 # 设置旋转变换的 w 分量为 1.0
self.tf_broadcaster.sendTransform(t) # 发送变换
def main(): # 主函数
rclpy.init() # 初始化 rclpy
node = FixedFrameBroadcaster() # 创建一个 FixedFrameBroadcaster 节点
try:
rclpy.spin(node) # 运行节点
except KeyboardInterrupt: # 捕捉键盘中断异常
pass
rclpy.shutdown() # 关闭 rclpy
代码与 tf2 广播器教程示例非常相似,唯一的区别是这里的变换不会随时间变化。
1.1 检查代码
让我们看看这段代码中的关键行。这里我们创建了一个新的变换,从父 turtle1
到新的子 carrot1
。 carrot1
框架在 y 轴上相对于 turtle1
框架偏移了 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.0
1.2 添加入口点
要允许 ros2 run
命令运行您的节点,您必须将入口点添加到 setup.py
(位于 src/learning_tf2_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
的新文件,并添加以下几行代码:
import os # 导入os模块,用于处理文件和目录
from ament_index_python.packages import get_package_share_directory # 导入get_package_share_directory函数,用于获取包的共享目录
from launch import LaunchDescription # 导入LaunchDescription类,用于描述启动配置
from launch.actions import IncludeLaunchDescription # 导入IncludeLaunchDescription类,用于包含其他的启动描述
from launch.launch_description_sources import PythonLaunchDescriptionSource # 导入PythonLaunchDescriptionSource类,用于从Python文件加载启动描述
from launch_ros.actions import Node # 导入Node类,用于描述ROS节点
def generate_launch_description(): # 定义一个名为generate_launch_description的函数
demo_nodes = IncludeLaunchDescription( # 创建一个IncludeLaunchDescription对象
PythonLaunchDescriptionSource([os.path.join( # 使用os.path.join函数拼接路径
get_package_share_directory('learning_tf2_py'), 'launch'), # 获取'learning_tf2_py'包的共享目录,并加上'launch'
'/turtle_tf2_demo_launch.py']), # 加上'/turtle_tf2_demo_launch.py'
)
return LaunchDescription([ # 返回一个LaunchDescription对象
demo_nodes, # 包含demo_nodes
Node( # 创建一个Node对象
package='learning_tf2_py', # 设置包名为'learning_tf2_py'
executable='fixed_frame_tf2_broadcaster', # 设置可执行文件名为'fixed_frame_tf2_broadcaster'
name='fixed_broadcaster', # 设置节点名为'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 jazzy -y
仍在工作区的根目录下,构建您的包:
colcon build --packages-select learning_tf2_py
打开一个新的终端,导航到工作区的根目录,然后加载设置文件:
. install/setup.bash
1.5 运行
现在你可以开始乌龟广播演示了
ros2 launch learning_tf2_py turtle_tf2_fixed_frame_demo_launch.py
您应该注意到新的 carrot1
框架出现在变换树中。
ros2 run tf2_tools view_frames
如果你驾驶第一只乌龟,你应该注意到行为没有从以前的教程中改变,即使我们添加了一个新的框架。那是因为添加一个额外的框架不会影响其他框架,我们的监听器仍然使用先前定义的框架。
因此,如果我们希望第二只乌龟跟随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
,你会看到第二只乌龟跟着胡萝卜carrot1而不是第一只乌龟!
2 编写动态帧广播器
我们在本教程中发布的额外帧是一个固定帧,它不会随时间相对于父帧发生变化。然而,如果您想发布一个移动帧,可以编写广播器代码使帧随时间变化。让我们更改我们的 carrot1
帧,使其随时间相对于 turtle1
帧发生变化。转到我们在上一个教程中创建的 learning_tf2_py
包。在 src/learning_tf2_py/learning_tf2_py
目录中,通过输入以下命令下载动态帧广播器代码:
cxy@ubuntu2404-cxy:~/ros2_ws/src/learning_tf2_py/learning_tf2_py$ wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/dynamic_frame_tf2_broadcaster.py
现在打开名为 dynamic_frame_tf2_broadcaster.py
的文件:
import math # 导入 math 库
from geometry_msgs.msg import TransformStamped # 从 geometry_msgs.msg 导入 TransformStamped 消息类型
import rclpy # 导入 rclpy 库
from rclpy.node import Node # 从 rclpy.node 导入 Node 类
from tf2_ros import TransformBroadcaster # 从 tf2_ros 导入 TransformBroadcaster 类
class DynamicFrameBroadcaster(Node): # 定义一个名为 DynamicFrameBroadcaster 的类,继承自 Node 类
def __init__(self): # 初始化方法
super().__init__('dynamic_frame_tf2_broadcaster') # 调用父类的初始化方法,并设置节点名称为 'dynamic_frame_tf2_broadcaster'
self.tf_broadcaster = TransformBroadcaster(self) # 创建一个 TransformBroadcaster 对象
self.timer = self.create_timer(0.1, self.broadcast_timer_callback) # 创建一个定时器,每 0.1 秒调用一次 broadcast_timer_callback 方法
def broadcast_timer_callback(self): # 定义定时器回调方法
seconds, _ = self.get_clock().now().seconds_nanoseconds() # 获取当前时间的秒数
x = seconds * math.pi # 计算 x 值
t = TransformStamped() # 创建一个 TransformStamped 对象
t.header.stamp = self.get_clock().now().to_msg() # 设置时间戳为当前时间
t.header.frame_id = 'turtle1' # 设置父坐标系 ID 为 'turtle1'
t.child_frame_id = 'carrot1' # 设置子坐标系 ID 为 'carrot1'
t.transform.translation.x = 10 * math.sin(x) # 设置平移变换的 x 坐标为 10 * sin(x)
t.transform.translation.y = 10 * math.cos(x) # 设置平移变换的 y 坐标为 10 * cos(x)
t.transform.translation.z = 0.0 # 设置平移变换的 z 坐标为 0.0
t.transform.rotation.x = 0.0 # 设置旋转变换的 x 分量为 0.0
t.transform.rotation.y = 0.0 # 设置旋转变换的 y 分量为 0.0
t.transform.rotation.z = 0.0 # 设置旋转变换的 z 分量为 0.0
t.transform.rotation.w = 1.0 # 设置旋转变换的 w 分量为 1.0
self.tf_broadcaster.sendTransform(t) # 发送变换
def main(): # 主函数
rclpy.init() # 初始化 rclpy
node = DynamicFrameBroadcaster() # 创建一个 DynamicFrameBroadcaster 节点
try:
rclpy.spin(node) # 运行节点
except KeyboardInterrupt: # 捕捉键盘中断异常
pass
rclpy.shutdown() # 关闭 rclpy
2.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
命令运行您的节点,您必须将入口点添加到 setup.py
(位于 src/learning_tf2_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
,并粘贴以下代码:
import os # 导入 os 模块
from ament_index_python.packages import get_package_share_directory # 从 ament_index_python.packages 导入 get_package_share_directory 函数
from launch import LaunchDescription # 从 launch 导入 LaunchDescription 类
from launch.actions import IncludeLaunchDescription # 从 launch.actions 导入 IncludeLaunchDescription 类
from launch.launch_description_sources import PythonLaunchDescriptionSource # 从 launch.launch_description_sources 导入 PythonLaunchDescriptionSource 类
from launch_ros.actions import Node # 从 launch_ros.actions 导入 Node 类
def generate_launch_description(): # 定义 generate_launch_description 函数
demo_nodes = IncludeLaunchDescription( # 创建 IncludeLaunchDescription 对象
PythonLaunchDescriptionSource([os.path.join( # 使用 PythonLaunchDescriptionSource 指定启动文件路径
get_package_share_directory('learning_tf2_py'), 'launch'), # 获取 'learning_tf2_py' 包的共享目录,并指定 'launch' 文件夹
'/turtle_tf2_demo_launch.py']), # 指定启动文件名
launch_arguments={'target_frame': 'carrot1'}.items(), # 设置启动参数 'target_frame' 为 'carrot1'
)
return LaunchDescription([ # 返回 LaunchDescription 对象
demo_nodes, # 包含 demo_nodes
Node( # 创建 Node 对象
package='learning_tf2_py', # 指定包名为 'learning_tf2_py'
executable='dynamic_frame_tf2_broadcaster', # 指定可执行文件名为 'dynamic_frame_tf2_broadcaster'
name='dynamic_broadcaster', # 设置节点名称为 'dynamic_broadcaster'
),
])
2.4 构建
在工作区的根目录运行 rosdep
以检查缺失的依赖项。
rosdep install -i --from-path src --rosdistro jazzy -y
仍在工作区的根目录下,构建您的包:
colcon build --packages-select learning_tf2_py
打开一个新的终端,导航到工作区的根目录,然后加载设置文件:
. install/setup.bash
1.5 运行
现在你可以开始动态框架演示了:
ros2 launch learning_tf2_py turtle_tf2_dynamic_frame_demo_launch.py
你应该看到第二只乌龟正在跟随不断变化的胡萝卜的位置。
摘要
在本教程中,您了解了 tf2 变换树、其结构及其特性。您还了解到,在本地框架内思考是最容易的,并学会了为该本地框架添加额外的固定和动态框架。