目标：学习如何向 tf2 添加额外的框架。
教程级别：中级
时间：15 分钟
目录
背景
tf2 树
任务
1 编写固定帧广播器
1.1 检查代码
1.2 CMakeLists.txt
1.3 编写启动文件
1.4 构建
1.5 运行
2 编写动态帧广播器
2.1 检查代码
2.2 CMakeLists.txt
2.3 编写启动文件
2.4 构建
2.5 运行
摘要
背景
在之前的教程中，我们通过编写 tf2 广播器 https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.html 和 tf2 监听器 https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Listener-Cpp.html 重新创建了乌龟演示。本教程将教您如何向变换树添加额外的固定和动态帧。事实上，在 tf2 中添加帧与创建 tf2 广播器非常相似，但本示例将向您展示 tf2 的一些附加功能。
对于许多与变换相关的任务，在局部坐标系中思考更容易。例如，最容易在激光扫描仪中心的坐标系中推理激光扫描测量值。tf2 允许您为系统中的每个传感器、链接或关节定义一个局部坐标系。当从一个坐标系变换到另一个坐标系时，tf2 将处理所有引入的隐藏中间坐标系变换。
tf2 树 🌳
tf2 构建了一个帧的树结构，因此不允许帧结构中存在闭环。这意味着一个帧只有一个父帧，但它可以有多个子帧。目前，我们的 tf2 树包含三个帧： world 、 turtle1 和 turtle2 。两个乌龟帧是 world 帧的子帧。如果我们想向 tf2 添加一个新帧，三个现有帧中的一个需要成为父帧，而新帧将成为其子帧。
任务
1 编写固定帧广播器
在我们的乌龟示例中，我们将添加一个新的框架 carrot1 ，它将是 turtle1 的子框架。这个框架将作为第二只乌龟的目标。
首先创建源文件。进入我们在前面的教程中创建的 learning_tf2_cpp 包。在 src 目录中，通过输入以下命令下载固定帧广播代码：
cxy@ubuntu2404-cxy:~/ros2_ws/src/learning_tf2_cpp/src$ wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/fixed_frame_tf2_broadcaster.cpp现在打开名为 fixed_frame_tf2_broadcaster.cpp 的文件。
#include <chrono> // 导入 chrono 库
#include <functional> // 导入 functional 库
#include <memory> // 导入 memory 库
#include "geometry_msgs/msg/transform_stamped.hpp" // 导入 TransformStamped 消息类型
#include "rclcpp/rclcpp.hpp" // 导入 rclcpp 库
#include "tf2_ros/transform_broadcaster.h" // 导入 TransformBroadcaster 类
using namespace std::chrono_literals; // 使用 std::chrono_literals 命名空间
class FixedFrameBroadcaster : public rclcpp::Node // 定义一个名为 FixedFrameBroadcaster 的类，继承自 rclcpp::Node 类
{
public:
FixedFrameBroadcaster() // 构造函数
: Node("fixed_frame_tf2_broadcaster") // 调用父类构造函数，并设置节点名称为 "fixed_frame_tf2_broadcaster"
{
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this); // 创建一个 TransformBroadcaster 对象
timer_ = this->create_wall_timer( // 创建一个定时器
100ms, std::bind(&FixedFrameBroadcaster::broadcast_timer_callback, this)); // 每 100 毫秒调用一次 broadcast_timer_callback 方法
}
private:
void broadcast_timer_callback() // 定义定时器回调方法
{
geometry_msgs::msg::TransformStamped t; // 创建一个 TransformStamped 对象
t.header.stamp = this->get_clock()->now(); // 设置时间戳为当前时间
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
tf_broadcaster_->sendTransform(t); // 发送变换
}
rclcpp::TimerBase::SharedPtr timer_; // 定义一个定时器指针
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_; // 定义一个 TransformBroadcaster 指针
};
int main(int argc, char * argv[]) // 主函数
{
rclcpp::init(argc, argv); // 初始化 rclcpp
rclcpp::spin(std::make_shared<FixedFrameBroadcaster>()); // 创建并运行 FixedFrameBroadcaster 节点
rclcpp::shutdown(); // 关闭 rclcpp
return 0; // 返回 0
}代码与 tf2 广播器教程示例非常相似，唯一的区别是这里的变换不会随时间变化。
1.1 检查代码
让我们看看这段代码中的关键行。这里我们创建了一个新的变换，从父 turtle1 到新的子 carrot1 。 carrot1 框架在 y 轴上相对于 turtle1 框架偏移了 2 米。
geometry_msgs::msg::TransformStamped t;
t.header.stamp = this->get_clock()->now();
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 CMakeLists.txt
返回到 learning_tf2_cpp 目录， CMakeLists.txt 和 package.xml 文件位于那里。
现在打开 CMakeLists.txt ，添加可执行文件并将其命名为 fixed_frame_tf2_broadcaster 。
add_executable(fixed_frame_tf2_broadcaster src/fixed_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
fixed_frame_tf2_broadcaster
geometry_msgs
rclcpp
tf2_ros
)最后，添加 install(TARGETS…) 部分，以便 ros2 run 可以找到你的可执行文件：
install(TARGETS
fixed_frame_tf2_broadcaster
DESTINATION lib/${PROJECT_NAME})1.3 编写启动文件
现在让我们为这个示例创建一个启动文件。使用您的文本编辑器，在 src/learning_tf2_cpp/launch 目录中创建一个名为 turtle_tf2_fixed_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_cpp'), 'launch'), # 获取 'learning_tf2_cpp' 包的共享目录，并指定 'launch' 文件夹
'/turtle_tf2_demo_launch.py']), # 指定启动文件名
)
return LaunchDescription([ # 返回 LaunchDescription 对象
demo_nodes, # 包含 demo_nodes
Node( # 创建 Node 对象
package='learning_tf2_cpp', # 指定包名为 'learning_tf2_cpp'
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_cpp',
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_cpp打开一个新的终端，导航到工作区的根目录，然后加载设置文件：
. install/setup.bash1.5 运行
现在你可以开始乌龟广播演示了
ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_launch.py您应该注意到新的 carrot1 框架出现在变换树中。
如果你驾驶第一只乌龟，你应该注意到行为没有从以前的教程中改变，即使我们添加了一个新的框架。那是因为添加一个额外的框架不会影响其他框架，我们的监听器仍然使用先前定义的框架。
因此，如果我们希望第二只乌龟跟随胡萝卜而不是第一只乌龟，我们需要更改 target_frame 的值。这可以通过两种方式完成。一种方法是直接从控制台将 target_frame 参数传递给启动文件：
ros2 launch learning_tf2_cpp 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_cpp 包。在 src 目录中，通过输入以下命令下载动态帧广播器代码：
cxy@ubuntu2404-cxy:~/ros2_ws/src/learning_tf2_cpp/src$ wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/dynamic_frame_tf2_broadcaster.cpp现在打开名为 dynamic_frame_tf2_broadcaster.cpp 的文件：
#include <chrono> // 导入C++标准库的时间库
#include <functional> // 导入C++标准库的函数库
#include <memory> // 导入C++标准库的内存库
#include "geometry_msgs/msg/transform_stamped.hpp" // 导入geometry_msgs包的TransformStamped消息
#include "rclcpp/rclcpp.hpp" // 导入ROS2的rclcpp库
#include "tf2_ros/transform_broadcaster.h" // 导入tf2_ros的TransformBroadcaster库
using namespace std::chrono_literals; // 使用std::chrono_literals命名空间
const double PI = 3.141592653589793238463; // 定义常量PI，表示圆周率
class DynamicFrameBroadcaster : public rclcpp::Node // 定义一个名为DynamicFrameBroadcaster的类，继承自rclcpp::Node
{
public:
DynamicFrameBroadcaster() // DynamicFrameBroadcaster类的构造函数
: Node("dynamic_frame_tf2_broadcaster") // 初始化节点名为"dynamic_frame_tf2_broadcaster"
{
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this); // 创建一个新的tf2_ros::TransformBroadcaster对象
timer_ = this->create_wall_timer( // 创建一个定时器
100ms, std::bind(&DynamicFrameBroadcaster::broadcast_timer_callback, this)); // 每100毫秒调用一次broadcast_timer_callback函数
}
private:
void broadcast_timer_callback() // 定义broadcast_timer_callback函数
{
rclcpp::Time now = this->get_clock()->now(); // 获取当前时间
double x = now.seconds() * PI; // 计算x的值
geometry_msgs::msg::TransformStamped t; // 创建一个TransformStamped消息
t.header.stamp = now; // 设置时间戳为当前时间
t.header.frame_id = "turtle1"; // 设置父帧ID为"turtle1"
t.child_frame_id = "carrot1"; // 设置子帧ID为"carrot1"
t.transform.translation.x = 10 * sin(x); // 设置x方向的平移为10*sin(x)
t.transform.translation.y = 10 * 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
tf_broadcaster_->sendTransform(t); // 发送TransformStamped消息
}
rclcpp::TimerBase::SharedPtr timer_; // 定义一个定时器
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_; // 定义一个tf2_ros::TransformBroadcaster对象
};
int main(int argc, char * argv[]) // 主函数
{
rclcpp::init(argc, argv); // 初始化ROS
rclcpp::spin(std::make_shared<DynamicFrameBroadcaster>()); // 创建DynamicFrameBroadcaster对象并开始处理ROS事件
rclcpp::shutdown(); // 关闭ROS
return 0; // 返回0表示程序正常结束
}2.1 检查代码
相对于我们 x 和 y 偏移量的固定定义，我们使用当前时间的 sin() 和 cos() 函数，因此 carrot1 的偏移量在不断变化。
double x = now.seconds() * PI;
...
t.transform.translation.x = 10 * sin(x);
t.transform.translation.y = 10 * cos(x);2.2 CMakeLists.txt
返回到 learning_tf2_cpp 目录， CMakeLists.txt 和 package.xml 文件位于那里。
现在打开 CMakeLists.txt ，添加可执行文件并将其命名为 dynamic_frame_tf2_broadcaster 。
add_executable(dynamic_frame_tf2_broadcaster src/dynamic_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
dynamic_frame_tf2_broadcaster
geometry_msgs
rclcpp
tf2_ros
)最后，添加 install(TARGETS…) 部分，以便 ros2 run 可以找到你的可执行文件：
install(TARGETS
dynamic_frame_tf2_broadcaster
DESTINATION lib/${PROJECT_NAME})2.3 编写启动文件
要测试此代码，请在 src/learning_tf2_cpp/launch 目录中创建一个新的启动文件 turtle_tf2_dynamic_frame_demo_launch.py ，并粘贴以下代码：
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
def generate_launch_description():
demo_nodes = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('learning_tf2_cpp'), 'launch'),
'/turtle_tf2_demo_launch.py']),
launch_arguments={'target_frame': 'carrot1'}.items(),
)
return LaunchDescription([
demo_nodes,
Node(
package='learning_tf2_cpp',
executable='dynamic_frame_tf2_broadcaster',
name='dynamic_broadcaster',
),
])2.4 构建
在工作区的根目录运行 rosdep 以检查缺失的依赖项。
rosdep install -i --from-path src --rosdistro jazzy -y仍在工作区的根目录下，构建您的包：
colcon build --packages-select learning_tf2_cpp打开一个新的终端，导航到工作区的根目录，然后加载设置文件：
. install/setup.bash2.5 运行
现在你可以开始动态框架演示了：
ros2 launch learning_tf2_cpp turtle_tf2_dynamic_frame_demo_launch.py你应该看到第二只乌龟正在跟随不断变化的胡萝卜的位置。
摘要
在本教程中，您了解了 tf2 变换树、其结构及其特性。您还了解到，在本地框架内思考是最容易的，并学会了为该本地框架添加额外的固定和动态框架。
扫一扫
1237
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
