1.0 turtlesim 包
turtlesim 是 ROS2 官方提供的一个经典教学包，它实现了一个简单的二维乌龟（Turtle）仿真器
1.1 turtlesim::控制乌龟移动
# 显示一个包含一只初始乌龟的窗口
rosrun turtlesim turtlesim_node
# ros2 topic pub <topic_name> <msg_type> <arg1> <arg2> ...
# 发布消息次数（一次）: -1/--once
# 消息类型：geometry_msgs/msg/Twist
# 消息内容（YAML格式）：
# -- x轴以 1.0 m/s 的线速度前进，y轴和z轴线速度为0，
# -- x, y, z 轴的角速度都为0。
ros2 topic pub -1 /turtle1/cmd_vel geometry_msgs/msg/Twist '{linear: {x: 1.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'
# 以n的频率发布消息：-r -次数
ros2 topic pub -r 次数 /turtle1/cmd_vel geometry_msgs/msg/Twist '{linear: {x: 1.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}'
1.2 turtlesim::检查可用的topic和服务Server
# 显示一个包含一只初始乌龟的窗口
rosrun turtlesim turtlesim_node
# 查看所有服务
ros2 service list
------------------------------
/clear
/kill
/reset
/spawn
/turtle1/set_pen
/turtle1/teleport_absolute
/turtle1/teleport_relative
/turtlesim/describe_parameters
/turtlesim/get_parameter_types
/turtlesim/get_parameters
/turtlesim/list_parameters
/turtlesim/set_parameters
/turtlesim/set_parameters_atomically
------------------------------
# 查看特定节点提供的服务
ros2 node info /turtlesim
-------------------------------
/turtlesim
Subscribers:
/parameter_events: rcl_interfaces/msg/ParameterEvent
/turtle1/cmd_vel: geometry_msgs/msg/Twist
Publishers:
/parameter_events: rcl_interfaces/msg/ParameterEvent
/rosout: rcl_interfaces/msg/Log
/turtle1/color_sensor: turtlesim/msg/Color
/turtle1/pose: turtlesim/msg/Pose
Service Servers:
/clear: std_srvs/srv/Empty
/kill: turtlesim/srv/Kill
/reset: std_srvs/srv/Empty
/spawn: turtlesim/srv/Spawn
/turtle1/set_pen: turtlesim/srv/SetPen
/turtle1/teleport_absolute: turtlesim/srv/TeleportAbsolute
/turtle1/teleport_relative: turtlesim/srv/TeleportRelative
/turtlesim/describe_parameters: rcl_interfaces/srv/DescribeParameters
/turtlesim/get_parameter_types: rcl_interfaces/srv/GetParameterTypes
/turtlesim/get_parameters: rcl_interfaces/srv/GetParameters
/turtlesim/list_parameters: rcl_interfaces/srv/ListParameters
/turtlesim/set_parameters: rcl_interfaces/srv/SetParameters
/turtlesim/set_parameters_atomically: rcl_interfaces/srv/SetParametersAtomically
Service Clients:
Action Servers:
/turtle1/rotate_absolute: turtlesim/action/RotateAbsolute
Action Clients:
-------------------------------
1.3 检查topic消息类型和字段
ros2 topic type <topic_name>
-------------------------------
ros2 topic type /turtle1/cmd_vel
geometry_msgs/msg/Twist
-------------------------------
ros2 topic info <topic_name>
-------------------------------
ros2 topic info /turtle1/cmd_vel
Type: geometry_msgs/msg/Twist
Publisher count: 0
Subscription count: 1
-------------------------------
1.4 检查Server接口及字段
查看某个接口服务
ros2 service type /spawn
-------------------------------
turtlesim/srv/Spawn
-------------------------------
查看服务接口的详细定义:
ros2 interface show turtlesim/srv/Spawn
-------------------------------
float32 x
float32 y
float32 theta
string name
---
string name
-------------------------------
1.5 调用接口，生成第二只小乌龟
ros2 service call /spawn turtlesim/srv/Spawn "{name: 'turtle2', x: 2.0, y: 2.0, theta: 0.0}"
1.6 小乌龟坐标系
ros2 run learning_tf2_cpp turtle_tf2_broadcaster --ros-args -p turtlename:=turtle1
ros2 run learning_tf2_cpp turtle_tf2_broadcaster --ros-args -p turtlename:=turtle2
1. turtle1 和 turtle2 是 turtlesim 仿真器中的两个完全独立的、可移动的虚拟乌龟。
2. 共享环境: 它们都在同一个二维平面（蓝色窗口）内活动。
3. 独立的坐标系: 当你运行两个 turtle_tf2_broadcaster 节点时：
-- 创建一个名为 turtle1_frame 的坐标系
-- 创建一个名为 turtle2_frame 的坐标系
-- turtle1_frame 和 turtle2_frame 都以 world 作为它们的父坐标系。world 可以理解为仿真世界的绝对参考系。
-- 相对位置查询: 由于 turtle1_frame 和 turtle2_frame 都连接到了同一个 world 坐标系下，TF2 系统就能够计算出 turtle2_frame 相对于 turtle1_frame 的位置和方向。这就是 turtle_tf2_listener 能够实现追踪的关键。
4. 独立的控制: turtlesim_node 为每只乌龟提供独立的话题接口：
-- 可以通过向 /turtle1/cmd_vel 发布消息来控制 turtle1
-- 可以通过向 /turtle2/cmd_vel 发布消息来控制 turtle2
5. 独立的位姿: turtlesim_node 也为每只乌龟发布独立的位姿信息：
-- turtle1 的位姿通过 /turtle1/pose 话题发布
-- turtle2 的位姿通过 /turtle2/pose 话题发布
1.7 turtlesim、turtle_tf2_broadcaster、listener间关系
1. turtlesim 负责模拟 turtle1 和 turtle2 的物理行为，并提供它们的位姿数据 (/turtle1/pose, /turtle2/pose)。
2. turtle_tf2_broadcaster 节点分别监听这些位姿数据，并将每只乌龟的位置和方向转换为一个独立的 TF 坐标系 (turtle1_frame, turtle2_frame)，并发布到 TF 树中。
3. turtle_tf2_listener 节点则利用 TF2 库的强大功能，查询 turtle2_frame 相对于 turtle1_frame 的变换（即 turtle2 在 turtle1 坐标系下的位置 (x, y)）。根据这个相对位置，listener 节点计算出 turtle2 需要的运动指令 (Twist)，并发布到 /turtle2/cmd_vel 话题，从而驱动 turtle2 去追赶 turtle1。
总结核心要点:
turtle1 和 turtle2 本身是独立的。
但通过 TF2 框架，我们为它们各自创建了坐标系 (turtle1_frame, turtle2_frame)，并将这些坐标系链接到一个公共的参考系 (world) 下。
这就使得我们可以方便地计算它们之间的相对关系，从而实现像“追踪”这样的高级功能。
1.8 小乌龟跟随
ros2 pkg create learning_tf2_cpp --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs turtlesim
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>learning_tf2_cpp</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="bk@todo.todo">bk</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>geometry_msgs</depend>
<depend>turtlesim</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
cmake_minimum_required(VERSION 3.8)
project(learning_tf2_cpp)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(turtlesim REQUIRED)
# --- 添加可执行文件定义 ---
add_executable(turtle_tf2_broadcaster src/turtle_tf2_broadcaster.cpp)
ament_target_dependencies(turtle_tf2_broadcaster
rclcpp tf2 tf2_ros geometry_msgs turtlesim)
add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)
ament_target_dependencies(turtle_tf2_listener
rclcpp tf2 tf2_ros geometry_msgs turtlesim)
# Install executables
install(TARGETS
turtle_tf2_broadcaster
turtle_tf2_listener
DESTINATION lib/${PROJECT_NAME}
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()
// === 头文件包含 ===
// 用于处理 TF 变换相关的消息类型
#include <geometry_msgs/msg/transform_stamped.hpp>
// ROS2 C++ 客户端库的核心头文件
#include <rclcpp/rclcpp.hpp>
// TF2 库，用于处理旋转（四元数）
#include <tf2/LinearMath/Quaternion.h>
// TF2 ROS 包装库，用于发布坐标变换
#include <tf2_ros/transform_broadcaster.h>
// 用于订阅 Turtlesim 的位姿消息类型
#include "turtlesim/msg/pose.hpp"
// === 定义广播器类 ===
// 继承自 rclcpp::Node，这是一个 ROS2 节点
class FramePublisher : public rclcpp::Node
{
public:
// 构造函数
FramePublisher()
: Node("turtle_tf2_broadcaster") // 调用父类构造函数，设置节点名称为 "turtle_tf2_broadcaster"
{
// --- 1. 声明并获取参数 ---
// 声明一个名为 "turtlename" 的参数，类型为字符串，如果用户没有指定，则默认值为 "turtle1"
this->declare_parameter<std::string>("turtlename", "turtle1");
// 从参数服务器获取 "turtlename" 参数的实际值
// 这个值由运行时的命令行参数 --ros-args -p turtlename:=... 指定
turtlename_ = this->get_parameter("turtlename").as_string();
// --- 2. 创建订阅者 ---
// 创建一个 Subscription，用于订阅乌龟的位姿信息
// 话题名称是动态构建的：turtlename_ + "/pose" (例如 "turtle1/pose")
// 消息队列大小为 1
// 当收到消息时，会调用 pose_callback 回调函数，并将消息作为参数传入
using std::placeholders::_1; // 用于 std::bind
subscription_ = this->create_subscription<turtlesim::msg::Pose>(
turtlename_ + "/pose", 1, // 动态构建话题名称
std::bind(&FramePublisher::pose_callback, this, _1) // 绑定回调函数
);
// --- 3. 创建 TransformBroadcaster ---
// 创建一个 TF2 TransformBroadcaster
// 它用于将坐标变换信息发布到 ROS2 的 /tf 话题上
// 需要传入当前节点的指针 (this)，以便它能正确地发布消息
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
}
private:
// --- 位姿回调函数 ---
// 这个函数会在每次从 turtlename_/pose 话题收到新的位姿消息时被调用
void pose_callback(const turtlesim::msg::Pose::SharedPtr msg)
{
// --- 4. 构建 TransformStamped 消息 ---
// 创建一个 geometry_msgs::msg::TransformStamped 消息
// 这个消息将包含一个坐标系相对于另一个坐标系的完整变换信息（位置和方向）
geometry_msgs::msg::TransformStamped t;
// 设置消息头
// stamp: 设置时间戳，表示这个变换数据的生成时间
t.header.stamp = this->get_clock()->now();
// frame_id: 设置父坐标系的名称
t.header.frame_id = "world"; // 将 "world" 设为父坐标系，这是一个通用的参考系
// 设置子坐标系的名称
// child_frame_id: 设置子坐标系的名称，通常是动态构建的 (例如 "turtle1_frame")
t.child_frame_id = turtlename_ + "_frame";
// --- 5. 填充位置 (translation) 信息 ---
// 从 Turtlesim 的 Pose 消息中获取 x, y 坐标
// 并填充到 TransformStamped 消息的 translation 部分
t.transform.translation.x = msg->x; // x 轴位置
t.transform.translation.y = msg->y; // y 轴位置
t.transform.translation.z = 0.0; // z 轴位置，在 2D 平面仿真中始终为 0
// --- 6. 填充方向 (rotation) 信息 ---
// Turtlesim 的 Pose 消息中的 theta 是一个标量（弧度），表示朝向
// TF2 使用四元数 (quaternion) 来表示方向，所以我们需要转换
// 创建一个 tf2::Quaternion 对象
tf2::Quaternion q;
// 使用 setRPY 函数将欧拉角 (Roll, Pitch, Yaw) 转换为四元数
// 在 2D 平面中，只有 Yaw (偏航角) 有意义，所以 R 和 P 设为 0
// msg->theta 就是我们需要的 Yaw 角
q.setRPY(0, 0, msg->theta);
// 将计算出的四元数各分量赋值给 TransformStamped 消息的 rotation 部分
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();
// --- 7. 发布变换 ---
// 使用之前创建的 TransformBroadcaster 将构建好的变换消息发布出去
// 这个消息会被发送到 /tf 话题上，供其他节点（如 turtle_tf2_listener）使用
tf_broadcaster_->sendTransform(t);
}
// --- 成员变量声明 ---
// 存储订阅者，用于监听乌龟的位姿
rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
// 存储 TransformBroadcaster，用于发布坐标系变换
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
// 存储乌龟的名称（例如 "turtle1" 或 "turtle2"），用于构建话题名和坐标系名
std::string turtlename_;
};
// === 主函数 ===
int main(int argc, char * argv[])
{
// 初始化 ROS2 客户端库
rclcpp::init(argc, argv);
// 创建 FramePublisher 节点的实例并将其传递给 rclcpp::spin
// spin 函数会进入事件循环，处理订阅的消息（触发 pose_callback）、定时器等
rclcpp::spin(std::make_shared<FramePublisher>());
// 关闭 ROS2 客户端库
rclcpp::shutdown();
return 0;
}
// === 头文件包含 ===
// 用于处理 TF 变换相关的消息类型
#include <geometry_msgs/msg/transform_stamped.hpp>
// 用于发送速度指令的消息类型
#include <geometry_msgs/msg/twist.hpp>
// ROS2 C++ 客户端库的核心头文件
#include <rclcpp/rclcpp.hpp>
// TF2 库，用于处理旋转（四元数）
#include <tf2/LinearMath/Quaternion.h>
// TF2 ROS 包装库，用于缓存和监听变换
#include <tf2_ros/buffer.h>
#include <tf2_ros/create_timer_ros.h> // 用于 TF2 内部计时器
#include <tf2_ros/transform_listener.h> // TF2 监听器类
// 用于调用 Turtlesim 服务（本例中未实际使用，但可能在其他变体中用到）
#include "turtlesim/srv/spawn.hpp"
// === 定义监听器类 ===
// 继承自 rclcpp::Node，这是一个 ROS2 节点
class TurtleTF2Listener : public rclcpp::Node
{
public:
// 构造函数
TurtleTF2Listener()
: Node("turtle_tf2_listener") // 调用父类构造函数，设置节点名称为 "turtle_tf2_listener"
{
// --- 1. 初始化 TF2 Buffer 和 Listener ---
// 创建一个 TF2 Buffer，用于缓存从 /tf 话题接收到的坐标变换数据
// get_clock() 提供时钟，以便 Buffer 能够处理带时间戳的变换
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
// 创建一个 TF2 的计时器接口，这是 TF2 内部用于异步操作（如超时）所需的
// 它需要访问节点的基础接口和定时器接口
auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
this->get_node_base_interface(), // 节点基础接口（用于节点名称等）
this->get_node_timers_interface()); // 节点定时器接口（用于创建内部定时器）
// 将创建好的计时器接口设置给 Buffer
tf_buffer_->setCreateTimerInterface(timer_interface);
// 创建一个 TF2 TransformListener
// 它会自动订阅 /tf 和 /tf_static 话题，并将收到的变换数据存储到 tf_buffer_ 中
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
// --- 2. 创建速度指令发布者 ---
// 创建一个 Publisher，用于向 Turtlesim 发送速度指令
// 话题名称是 "/turtle2/cmd_vel"，队列大小为 1
cmd_vel_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
"/turtle2/cmd_vel", 1);
// --- 3. 创建定时器 ---
// 创建一个 Wall Timer（基于系统时间的定时器）
// 每隔 50 毫秒（即频率约为 20 Hz）执行一次 lookup_loop 函数
using namespace std::chrono_literals; // 方便使用 ms, s 等时间字面量
timer_ = this->create_wall_timer(
50ms, // 定时器间隔
std::bind(&TurtleTF2Listener::lookup_loop, this) // 绑定回调函数到当前对象
);
}
private:
// --- 核心循环函数 ---
// 这个函数会被定时器定期调用，执行查询变换和控制逻辑
void lookup_loop()
{
// 定义源坐标系和目标坐标系
// 注意：这里的名称必须与 TF 树中存在的坐标系名称完全一致
const std::string target_frame = "turtle2_frame"; // 目标坐标系，我们想知道它相对于源坐标系的位置
const std::string source_frame = "turtle1_frame"; // 源坐标系，作为参考坐标系
try {
// --- 4. 查询坐标变换 ---
// 使用 tf_buffer_ 查询从 source_frame 到 target_frame 的变换
// lookupTransform(target_frame, source_frame, time_point)
// tf2::TimePointZero 表示查询最新可用的变换（不关心具体时间戳）
geometry_msgs::msg::TransformStamped transformStamped = tf_buffer_->lookupTransform(
target_frame, source_frame, tf2::TimePointZero);
// --- 5. 提取位置信息 ---
// 从查询到的变换中提取目标坐标系 (turtle2_frame) 相对于源坐标系 (turtle1_frame) 的位置
// translation.x 和 translation.y 就是 turtle2 在 turtle1 坐标系下的 x, y 坐标
double x = transformStamped.transform.translation.x;
double y = transformStamped.transform.translation.y;
// --- 6. 控制逻辑：计算速度指令 ---
// 创建一个 Twist 消息，用于存储线速度和角速度
geometry_msgs::msg::Twist twist;
// 计算角速度 (angular.z)，使 turtle2 朝向 turtle1
// atan2(y, x) 计算从 turtle2 指向 turtle1 的角度（弧度）
// 乘以 4.0 是一个增益系数，用于加快转向速度
twist.angular.z = 4.0 * atan2(y, x);
// 计算线速度 (linear.x)，使 turtle2 向 turtle1 移动
// sqrt(x^2 + y^2) 计算 turtle2 到 turtle1 的直线距离
// 乘以 0.5 是一个增益系数，用于设定线速度与距离的比例
twist.linear.x = 0.5 * sqrt(pow(x, 2) + pow(y, 2));
// --- 7. 发布速度指令 ---
// 将计算好的速度指令发布到 "/turtle2/cmd_vel" 话题
// Turtlesim 节点会订阅这个话题，并驱动 turtle2 按照指令移动
cmd_vel_publisher_->publish(twist);
} catch (const tf2::TransformException & ex) { // 捕获 TF2 变换异常
// 如果查询失败（例如，指定的坐标系不存在，或缓冲区中没有足够新的数据）
// 打印错误信息到日志，便于调试
RCLCPP_INFO(
this->get_logger(), // 获取当前节点的日志记录器
"Could not transform %s to %s: %s", // 错误信息模板
target_frame.c_str(), source_frame.c_str(), ex.what()); // 填充模板变量
}
}
// --- 成员变量声明 ---
// 存储 TF2 缓冲区，用于缓存变换数据
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
// 存储 TF2 监听器，自动接收并更新变换数据到缓冲区
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
// 存储速度指令发布者，用于向 turtle2 发送运动指令
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_publisher_;
// 存储定时器，用于周期性执行 lookup_loop
rclcpp::TimerBase::SharedPtr timer_;
};
// === 主函数 ===
int main(int argc, char * argv[])
{
// 初始化 ROS2 客户端库
rclcpp::init(argc, argv);
// 创建 TurtleTF2Listener 节点的实例并将其传递给 rclcpp::spin
// spin 函数会进入事件循环，处理订阅的消息、定时器回调等
rclcpp::spin(std::make_shared<TurtleTF2Listener>());
// 关闭 ROS2 客户端库
rclcpp::shutdown();
return 0;
}
1.9 小乌龟跟随运行验证
【终端 1】启动 Turtlesim 仿真器:
ros2 run turtlesim turtlesim_node
【终端 2】生成第二只乌龟 turtle2:
ros2 service call /spawn turtlesim/srv/Spawn "{name: 'turtle2', x: 2.0, y: 2.0, theta: 0.0}"
【终端 3】启动 turtle1 的 TF 广播器:
ros2 run learning_tf2_cpp turtle_tf2_broadcaster --ros-args -p turtlename:=turtle1
【终端 4】启动 turtle2 的 TF 广播器:
ros2 run learning_tf2_cpp turtle_tf2_broadcaster --ros-args -p turtlename:=turtle2
【终端 5】最后启动 TF 监听器和控制器:
ros2 run learning_tf2_cpp turtle_tf2_listener
浙公网安备 33010602011771号