目标：学习如何将机器人的状态广播到 tf2。
教程级别：中级
时间：15 分钟
目录
背景
先决条件
任务
1. 编写广播器节点
2. 编写启动文件
3. 构建
4. 运行
摘要
背景
在接下来的两个教程中，我们将编写代码来复现“tf2 入门”教程中的演示。之后，后续教程将专注于使用更高级的 tf2 功能来扩展演示，包括在变换查找和时间旅行中使用超时。
先决条件
本教程假设您具有 ROS 2 的工作知识，并且您已经完成了 tf2 教程 https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Introduction-To-Tf2.html 和 tf2 静态广播器教程（C++）https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Static-Broadcaster-Cpp.html 的介绍。我们将重用上一个教程中的 learning_tf2_cpp 包。
在之前的教程中，您学习了如何创建工作区和创建包。
任务
1. 编写广播器节点
让我们先创建源文件。转到我们在上一教程中创建的 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/turtle_tf2_broadcaster.cpp使用您喜欢的文本编辑器打开文件。
// 版权 2021 开源机器人基金会
//
// 根据 Apache 许可证 2.0 版（“许可证”）授权；
// 除非符合许可证，否则您不得使用此文件。
// 您可以在以下网址获取许可证副本：
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// 除非适用法律要求或书面同意，否则根据许可证分发的软件
// 是按“原样”分发的，不附带任何明示或暗示的担保或条件。
// 请参阅许可证以了解管理权限和限制的特定语言。
#include <functional> // 导入功能性库
#include <memory> // 导入内存管理库
#include <sstream> // 导入字符串流库
#include <string> // 导入字符串库
#include "geometry_msgs/msg/transform_stamped.hpp" // 导入TransformStamped消息类型
#include "rclcpp/rclcpp.hpp" // 导入rclcpp库
#include "tf2/LinearMath/Quaternion.h" // 导入tf2四元数库
#include "tf2_ros/transform_broadcaster.h" // 导入tf2变换广播器库
#include "turtlesim/msg/pose.hpp" // 导入Pose消息类型
class FramePublisher : public rclcpp::Node // 定义FramePublisher类，继承自rclcpp::Node
{
public:
FramePublisher() // 构造函数
: Node("turtle_tf2_frame_publisher") // 调用父类构造函数，初始化节点名称为"turtle_tf2_frame_publisher"
{
// 声明并获取`turtlename`参数
turtlename_ = this->declare_parameter<std::string>("turtlename", "turtle");
// 初始化变换广播器
tf_broadcaster_ =
std::make_unique<tf2_ros::TransformBroadcaster>(*this);
// 订阅turtle{1}{2}/pose主题，并在每个消息上调用handle_turtle_pose回调函数
std::ostringstream stream; // 创建字符串流对象
stream << "/" << turtlename_.c_str() << "/pose"; // 构建主题名称
std::string topic_name = stream.str(); // 获取主题名称字符串
auto handle_turtle_pose =[this](const std::shared_ptr<turtlesim::msg::Pose> msg) { // 定义回调函数
geometry_msgs::msg::TransformStamped t; // 创建TransformStamped对象
// 读取消息内容并将其分配给相应的tf变量
t.header.stamp = this->get_clock()->now(); // 设置时间戳
t.header.frame_id = "world"; // 设置父坐标系ID
t.child_frame_id = turtlename_.c_str(); // 设置子坐标系ID
// 乌龟只存在于2D，因此我们从消息中获取x和y平移坐标，并将z坐标设置为0
t.transform.translation.x = msg->x; // 设置x平移坐标
t.transform.translation.y = msg->y; // 设置y平移坐标
t.transform.translation.z = 0.0; // 设置z平移坐标为0
// 出于同样的原因，乌龟只能绕一个轴旋转，这就是为什么我们将x和y的旋转设置为0，并从消息中获取z轴的旋转
tf2::Quaternion q; // 创建四元数对象
q.setRPY(0, 0, msg->theta); // 设置四元数的RPY值
t.transform.rotation.x = q.x(); // 设置四元数的x分量
t.transform.rotation.y = q.y(); // 设置四元数的y分量
t.transform.rotation.z = q.z(); // 设置四元数的z分量
t.transform.rotation.w = q.w(); // 设置四元数的w分量
// 发送变换
tf_broadcaster_->sendTransform(t);
};
subscription_ = this->create_subscription<turtlesim::msg::Pose>( // 创建订阅
topic_name, 10, // 订阅主题名称和队列大小
handle_turtle_pose); // 设置回调函数
}
private:
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_; // 声明订阅对象
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_; // 声明变换广播器对象
std::string turtlename_; // 声明乌龟名称参数
};
int main(int argc, char * argv[]) // 主函数
{
rclcpp::init(argc, argv); // 初始化rclcpp
rclcpp::spin(std::make_shared<FramePublisher>()); // 创建并运行FramePublisher节点
rclcpp::shutdown(); // 关闭rclcpp
return 0; // 返回0
}1.1 检查代码
现在，让我们来看一下与发布乌龟姿态到 tf2 相关的代码。首先，我们定义并获取一个参数 turtlename ，它指定了一个乌龟的名称，例如 turtle1 或 turtle2 。
turtlename_ = this->declare_parameter<std::string>("turtlename", "turtle");随后，节点订阅主题 turtleX/pose 并在每条传入消息上运行函数 handle_turtle_pose 。
subscription_ = this->create_subscription<turtlesim::msg::Pose>(
topic_name, 10,
handle_turtle_pose);现在，我们创建一个 TransformStamped 对象，并为其提供适当的元数据。
我们需要为正在发布的转换加上一个时间戳，我们将通过调用
this->get_clock()->now()来用当前时间来标记它。这将返回Node使用的当前时间。然后我们需要设置我们正在创建的链接的父框架的名称，在这种情况下是
world。最后，我们需要设置我们正在创建的链接的子节点的名称，在这种情况下，这就是乌龟本身的名称。
处理函数为 turtle 姿态消息广播此 turtle 的平移和旋转，并将其作为从框架 world 到框架 turtleX 的变换进行发布。
geometry_msgs::msg::TransformStamped t;
// Read message content and assign it to
// corresponding tf variables
t.header.stamp = this->get_clock()->now();
t.header.frame_id = "world";
t.child_frame_id = turtlename_.c_str();在这里，我们将 3D 乌龟姿势的信息复制到 3D 变换中。
// Turtle only exists in 2D, thus we get x and y translation
// coordinates from the message and set the z coordinate to 0
t.transform.translation.x = msg->x;
t.transform.translation.y = msg->y;
t.transform.translation.z = 0.0;
// For the same reason, turtle can only rotate around one axis
// and this why we set rotation in x and y to 0 and obtain
// rotation in z axis from the message
tf2::Quaternion q;
q.setRPY(0, 0, msg->theta);
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();最后我们将构建的转换传递给 TransformBroadcaster 的 sendTransform 方法，由它来负责广播。
// Send the transformation
tf_broadcaster_->sendTransform(t);1.2 CMakeLists.txt
返回到 learning_tf2_cpp 目录， CMakeLists.txt 和 package.xml 文件位于那里。
现在打开 CMakeLists.txt ，添加可执行文件并将其命名为 turtle_tf2_broadcaster ，稍后您将与 ros2 run 一起使用。
add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
turtle_tf2_broadcaster
geometry_msgs
rclcpp
tf2
tf2_ros
turtlesim
)最后，添加 install(TARGETS…) 部分，以便 ros2 run 可以找到你的可执行文件：
install(TARGETS
turtle_tf2_broadcaster
DESTINATION lib/${PROJECT_NAME})// 设置CMake的最低版本要求为3.8
cmake_minimum_required(VERSION 3.8) // 设置CMake的最低版本要求为3.8
// 定义一个名为learning_tf2_cpp的项目
project(learning_tf2_cpp) // 定义一个名为learning_tf2_cpp的项目
// 如果编译器是GNU或Clang，添加编译选项
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic) // 添加编译选项
endif()
// 寻找依赖项
find_package(ament_cmake REQUIRED) // 寻找ament_cmake包
find_package(geometry_msgs REQUIRED) // 寻找geometry_msgs包
find_package(rclcpp REQUIRED) // 寻找rclcpp包
find_package(tf2 REQUIRED) // 寻找tf2包
find_package(tf2_ros REQUIRED) // 寻找tf2_ros包
find_package(turtlesim REQUIRED) // 寻找turtlesim包
// 添加可执行文件
add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp) // 添加名为static_turtle_tf2_broadcaster的可执行文件
// 添加目标依赖项
ament_target_dependencies(
static_turtle_tf2_broadcaster // 目标文件
geometry_msgs // 依赖项
rclcpp // 依赖项
tf2 // 依赖项
tf2_ros // 依赖项
)
// 安装目标文件
install(TARGETS
static_turtle_tf2_broadcaster // 目标文件
DESTINATION lib/${PROJECT_NAME}) // 安装位置
// 添加可执行文件
add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp) // 添加名为turtle_tf2_broadcaster的可执行文件
// 添加目标依赖项
ament_target_dependencies(
turtle_tf2_broadcaster // 目标文件
geometry_msgs // 依赖项
rclcpp // 依赖项
tf2 // 依赖项
tf2_ros // 依赖项
turtlesim // 依赖项
)
// 安装目标文件
install(TARGETS
turtle_tf2_broadcaster // 目标文件
DESTINATION lib/${PROJECT_NAME}) // 安装位置
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME})
// 如果构建测试
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED) // 寻找ament_lint_auto包
// 跳过检查版权的linter
set(ament_cmake_copyright_FOUND TRUE) // 设置ament_cmake_copyright_FOUND为真
// 跳过cpplint（只在git仓库中工作）
set(ament_cmake_cpplint_FOUND TRUE) // 设置ament_cmake_cpplint_FOUND为真
ament_lint_auto_find_test_dependencies() // 自动寻找测试依赖项
endif()
// 打包
ament_package() // 打包2. 编写启动文件
现在为这个演示创建一个启动文件。在 src/learning_tf2_cpp 目录中创建一个 launch 文件夹。使用你的文本编辑器，在 launch 文件夹中创建一个名为 turtle_tf2_demo_launch.py 的新文件，并添加以下几行：
# 导入LaunchDescription模块
from launch import LaunchDescription
# 导入Node模块
from launch_ros.actions import Node
# 定义一个名为generate_launch_description的函数
def generate_launch_description():
# 返回一个LaunchDescription对象
return LaunchDescription([
# 创建一个Node对象
Node(
# 设置包名为'turtlesim'
package='turtlesim',
# 设置可执行文件名为'turtlesim_node'
executable='turtlesim_node',
# 设置节点名为'sim'
name='sim'
),
# 创建另一个Node对象
Node(
# 设置包名为'learning_tf2_cpp'
package='learning_tf2_cpp',
# 设置可执行文件名为'turtle_tf2_broadcaster'
executable='turtle_tf2_broadcaster',
# 设置节点名为'broadcaster1'
name='broadcaster1',
# 设置参数
parameters=[ // 设置参数
# 设置'turtlename'参数为'turtle1'
{'turtlename': 'turtle1'}
]
),
])2.1 检查代码
首先，我们从 launch 和 launch_ros 包中导入所需的模块。应当指出， launch 是一个通用的启动框架（不特定于 ROS 2），而 launch_ros 包含了 ROS 2 特定的内容，比如我们在这里导入的节点。
from launch import LaunchDescription
from launch_ros.actions import Node现在我们运行节点，启动 turtlesim 模拟并使用我们的 turtle_tf2_broadcaster 节点将 turtle1 状态广播到 tf2。
Node(
package='turtlesim',
executable='turtlesim_node',
name='sim'
),
Node(
package='learning_tf2_cpp',
executable='turtle_tf2_broadcaster',
name='broadcaster1',
parameters=[
{'turtlename': 'turtle1'}
]
),2.2 添加依赖项
返回到 learning_tf2_cpp 目录， CMakeLists.txt 和 package.xml 文件位于那里。
打开 package.xml 并使用文本编辑器。根据启动文件的导入语句添加以下依赖项：
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>此声明在执行其代码时需要额外的 launch 和 launch_ros 依赖项。
确保保存文件。
2.3 CMakeLists.txt
重新打开 CMakeLists.txt 并添加该行，以便安装 launch/ 文件夹中的启动文件。
install(DIRECTORY launch
DESTINATION share/${PROJECT_NAME})您可以在本教程中了解更多关于创建启动文件的信息。
3. 构建
在工作区的根目录运行 rosdep 以检查缺失的依赖项。
rosdep install -i --from-path src --rosdistro jazzy -y仍在工作区的根目录下，构建您的包：
colcon build --packages-select learning_tf2_cpp打开一个新的终端，导航到工作区的根目录，然后加载设置文件：
. install/setup.bash4. 运行
现在运行启动文件，它将启动 turtlesim 仿真节点和 turtle_tf2_broadcaster 节点：
ros2 launch learning_tf2_cpp turtle_tf2_demo_launch.py在第二个终端窗口中输入以下命令：
ros2 run turtlesim turtle_teleop_key您现在将看到 turtlesim 模拟已经开始，您可以控制一只乌龟。
现在，使用 tf2_echo 工具来检查海龟姿势是否真的正在向 tf2 广播：
ros2 run tf2_ros tf2_echo world turtle1这应该会显示第一只乌龟的姿势。使用箭头键绕着乌龟开（确保你的 turtle_teleop_key 终端窗口是活跃的，不是你的模拟器窗口）。在你的控制台输出中，你会看到类似这样的东西：
cxy@ubuntu2404-cxy:~/ros2_ws$ ros2 run tf2_ros tf2_echo world turtle1
[INFO] [1720692090.983150908] [tf2_echo]: Waiting for transform world -> turtle1: Invalid frame ID "world" passed to canTransform argument target_frame - frame does not exist
At time 1720692091.946408378
- Translation: [3.773, 3.598, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.992, 0.125]
- Rotation: in RPY (radian) [0.000, -0.000, 2.891]
- Rotation: in RPY (degree) [0.000, -0.000, 165.653]
- Matrix:
-0.969 -0.248 0.000 3.773
0.248 -0.969 0.000 3.598
0.000 0.000 1.000 0.000
0.000 0.000 0.000 1.000如果您运行 tf2_echo 以实现 world 和 turtle2 之间的转换，您不应该看到任何转换，因为第二只乌龟还没有出现。然而，一旦我们在下一个教程中添加了第二只乌龟， turtle2 的姿态将会广播到 tf2。
摘要
在本教程中，您学习了如何将机器人的姿态（乌龟的位置和方向）广播到 tf2，以及如何使用 tf2_echo 工具。为了实际使用广播到 tf2 的变换，您应该继续学习下一教程，关于创建 tf2 监听器 https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Listener-Cpp.html 。
扫一扫
1811
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
