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