【ROS2综合案例】乌龟跟随

一、前期准备 

1.1 安装 

        1. 首先安装“乌龟跟随”案例的功能包以及依赖项。

安装方式1（二进制方式安装）：

sudo apt-get install ros-humble-turtle-tf2-py ros-humble-tf2-tools ros-humble-tf-transformations

安装方式2（克隆源码并构建）：

git clone https://github.com/ros/geometry_tutorials.git -b ros2

        2. 还需要安装一个名为 transforms3d 的 Python 包，它为 tf_transformations包提供四元数和欧拉角变换功能，安装命令如下：

sudo apt install python3-pip
pip3 install transforms3d

1.2 执行

        1. 启动两个终端，终端1输入如下命令：该命令会启动 turtlesim_node 节点，turtlesim_node 节点中自带一只小乌龟 turtle1，除此之外还会新生成一只乌龟 turtle2，turtle2 会运行至 turtle1 的位置。

ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py

        2. 终端2输入如下命令：该终端下可以通过键盘控制 turtle1 运动，并且 turtle2 会跟随 turtle1 运动

ros2 run turtlesim turtle_teleop_key

1.3 坐标相关消息

        坐标变换的实现其本质是基于话题通信的发布订阅模型的，发布方可以发布坐标系之间的相对关系，订阅方则可以监听这些消息，并实现不同坐标系之间的变换。显然的根据之前介绍，在话题通信中，接口消息作为数据载体在整个通信模型中是比较重要的一部分，本节将会介绍坐标变换中常用的两种接口消息：

1.geometry_msgs/msg/TransformStamped    // 用于描述某一时刻两个坐标系之间相对关系的接口
2.geometry_msgs/msg/PointStamped        // 用于描述某一时刻坐标系内某个坐标点的位置的接口

        第一种：geometry_msgs/msg/TransformStamped 

通过如下命令查看接口定义：

ros2 interface show geometry_msgs/msg/TransformStamped

接口定义解释：

std_msgs/Header header
    builtin_interfaces/Time stamp     # 时间戳
        int32 sec
        uint32 nanosec
    string frame_id                   # 父级坐标系

string child_frame_id                 # 子级坐标系

Transform transform                   # 子级坐标系相对于父级坐标系的位姿
    Vector3 translation               # 三维偏移量
        float64 x
        float64 y
        float64 z
    Quaternion rotation               # 四元数
        float64 x 0
        float64 y 0
        float64 z 0
        float64 w 1

        第二种：geometry_msgs/msg/PointStamped

通过如下命令查看接口定义：

ros2 interface show geometry_msgs/msg/PointStamped

接口定义解释：

std_msgs/Header header
    builtin_interfaces/Time stamp    # 时间戳
        int32 sec
        uint32 nanosec
    string frame_id                  # 参考系
Point point                          # 三维坐标
    float64 x
    float64 y
    float64 z

二、乌龟跟随实例

        “乌龟跟随”案例的核心是如何确定 turtle1 相对 turtle2 的位置，只要该位置确定就可以把其作为目标点并控制 turtle2 向其运动。相对位置的确定可以通过坐标变换实现，大致思路是先分别广播 turtle1 相对于 world 和 turtle2 相对于 world 的坐标系关系，然后再通过监听坐标系关系进一步获取 turtle1 相对于 turtle2 的坐标系关系。

2.1 C++实现流程

2.1.1 准备工作

1.新建功能包：

ros2 pkg create cpp05_exercise --build-type ament_cmake --dependencies rclcpp tf2 tf2_ros geometry_msgs turtlesim

2.创建launch目录:

功能包cpp05_exercise下新建launch文件，并编辑配置文件。

功能包cpp05_exercise的 CMakeLists.txt 文件添加如下内容：

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

2.1.2 编写生成新乌龟

        功能包 cpp05_exercise 的 src 目录下，新建 C++ 文件 exer01_tf_spawn.cpp，并编辑文件，输入如下内容：

/*  
  需求：编写客户端，发送请求生成一只新的乌龟。
  步骤：
    1.包含头文件；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.声明并获取参数；
      3-2.创建客户端；
      3-3.等待服务连接；
      3-4.组织请求数据并发送；
    4.创建对象指针调用其功能,并处理响应；
    5.释放资源。

*/
// 1.包含头文件；
#include "rclcpp/rclcpp.hpp"
#include "turtlesim/srv/spawn.hpp"

using namespace std::chrono_literals;

// 3.定义节点类；
class TurtleSpawnClient: public rclcpp::Node{
  public:
    TurtleSpawnClient():Node("turtle_spawn_client"){
      // 3-1.声明并获取参数；
      this->declare_parameter("x",2.0);
      this->declare_parameter("y",8.0);
      this->declare_parameter("theta",0.0);
      this->declare_parameter("turtle_name","turtle2");
      x = this->get_parameter("x").as_double();
      y = this->get_parameter("y").as_double();
      theta = this->get_parameter("theta").as_double();
      turtle_name = this->get_parameter("turtle_name").as_string();
      // 3-2.创建客户端；
      client = this->create_client<turtlesim::srv::Spawn>("/spawn");
    }
    // 3-3.等待服务连接；
    bool connect_server(){
      while (!client->wait_for_service(1s))
      {
        if (!rclcpp::ok())
        {
          RCLCPP_INFO(this->get_logger(),"客户端退出！");
          return false;
        }

        RCLCPP_INFO(this->get_logger(),"服务连接中，请稍候...");
      }
      return true;
    }
    // 3-4.组织请求数据并发送；
    rclcpp::Client<turtlesim::srv::Spawn>::FutureAndRequestId send_request(){
      auto request = std::make_shared<turtlesim::srv::Spawn::Request>();
      request->x = x;
      request->y = y;
      request->theta = theta;
      request->name = turtle_name;
      return client->async_send_request(request);
    }


  private:
    rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr client;
    float_t x,y,theta;
    std::string turtle_name;
};

int main(int argc, char ** argv)
{
  // 2.初始化 ROS2 客户端；
  rclcpp::init(argc,argv);

  // 4.创建对象指针并调用其功能；
  auto client = std::make_shared<TurtleSpawnClient>();
  bool flag = client->connect_server();
  if (!flag)
  {
    RCLCPP_INFO(client->get_logger(),"服务连接失败!");
    return 0;
  }

  auto response = client->send_request();

  // 处理响应
  if (rclcpp::spin_until_future_complete(client,response) == rclcpp::FutureReturnCode::SUCCESS)
  {
    RCLCPP_INFO(client->get_logger(),"请求正常处理");
    std::string name = response.get()->name;
    if (name.empty())
    {
        RCLCPP_INFO(client->get_logger(),"乌龟重名导致生成失败！");
    } else {
        RCLCPP_INFO(client->get_logger(),"乌龟%s生成成功！", name.c_str());
    }

  } else {
    RCLCPP_INFO(client->get_logger(),"请求异常");
  }

  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

2.1.3 编写坐标变换广播

        功能包 cpp05_exercise 的 src 目录下，新建 C++ 文件 exer02_tf_broadcaster.cpp，并编辑文件，输入如下内容：

/*   
  需求：发布乌龟坐标系到窗口坐标系的坐标变换。
  步骤：
    1.包含头文件；
    2.初始化 ROS 客户端；
    3.定义节点类；
      3-1.声明并解析乌龟名称参数；
      3-2.创建动态坐标变换发布方；
      3-3.创建乌龟位姿订阅方；
      3-4.根据订阅到的乌龟位姿生成坐标帧并广播。
    4.调用 spin 函数，并传入对象指针；
    5.释放资源。

*/
// 1.包含头文件；
#include <geometry_msgs/msg/transform_stamped.hpp>

#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <turtlesim/msg/pose.hpp>

using std::placeholders::_1;

// 3.定义节点类；
class TurtleFrameBroadcaster : public rclcpp::Node
{
public:
  TurtleFrameBroadcaster(): Node("turtle_frame_broadcaster")
  {
    // 3-1.声明并解析乌龟名称参数；
    this->declare_parameter("turtle_name","turtle1");
    turtle_name = this->get_parameter("turtle_name").as_string();
    // 3-2.创建动态坐标变换发布方；
    tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
    std::string topic_name = turtle_name + "/pose";

    // 3-3.创建乌龟位姿订阅方；
    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
      topic_name, 10,
      std::bind(&TurtleFrameBroadcaster::handle_turtle_pose, this, _1));
  }

private:
  // 3-4.根据订阅到的乌龟位姿生成坐标帧并广播。   
  void handle_turtle_pose(const turtlesim::msg::Pose & msg)
  {
    // 组织消息
    geometry_msgs::msg::TransformStamped t;
    rclcpp::Time now = this->get_clock()->now();

    t.header.stamp = now;
    t.header.frame_id = "world";
    t.child_frame_id = turtle_name;

    t.transform.translation.x = msg.x;
    t.transform.translation.y = msg.y;
    t.transform.translation.z = 0.0;

    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();
    // 发布消息
    tf_broadcaster_->sendTransform(t);
  }
  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  std::string turtle_name;
};

int main(int argc, char * argv[])
{
  // 2.初始化 ROS 客户端；
  rclcpp::init(argc, argv);
  // 4.调用 spin 函数，并传入对象指针；
  rclcpp::spin(std::make_shared<TurtleFrameBroadcaster>());
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

2.1.4 编写坐标变换监听

        功能包 cpp05_exercise 的 src 目录下，新建 C++ 文件 exer03_tf_listener.cpp，并编辑文件，输入如下内容：

/*  
  需求：广播的坐标系消息，并生成 turtle2 相对于 turtle1 的坐标系数据，
       并进一步生成控制 turtle2 运动的速度指令。
  步骤：
    1.包含头文件；
    2.初始化 ROS 客户端；
    3.定义节点类；
      3-1.声明并解析参数；
      3-2.创建tf缓存对象指针；
      3-3.创建tf监听器；
      3-4.按照条件查找符合条件的坐标系并生成变换后的坐标帧。
      3-5.生成 turtle2 的速度指令，并发布。
    4.调用 spin 函数，并传入对象指针；
    5.释放资源。

*/
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/msg/twist.hpp"

using namespace std::chrono_literals;

// 3.定义节点类；
class TurtleFrameListener : public rclcpp::Node {
public:
  TurtleFrameListener():Node("turtle_frame_listener"){
    // 3-1.声明并解析参数；
    this->declare_parameter("target_frame","turtle2");
    this->declare_parameter("source_frame","turtle1");
    target_frame = this->get_parameter("target_frame").as_string();
    source_frame = this->get_parameter("source_frame").as_string();

    // 3-2.创建tf缓存对象指针；
    tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
    // 3-3.创建tf监听器；
    transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
    twist_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(target_frame + "/cmd_vel",10);
    timer_ = this->create_wall_timer(1s, std::bind(&TurtleFrameListener::on_timer,this));
  }

private:
  void on_timer(){
    // 3-4.按照条件查找符合条件的坐标系并生成变换后的坐标帧。
    geometry_msgs::msg::TransformStamped transformStamped;
    try
    {
      transformStamped = tf_buffer_->lookupTransform(target_frame,source_frame,tf2::TimePointZero);
    }
    catch(const tf2::LookupException& e)
    {
      RCLCPP_INFO(this->get_logger(),"坐标变换异常：%s",e.what());
      return;
    }
    // 3-5.生成 turtle2 的速度指令，并发布。
    geometry_msgs::msg::Twist msg;
    static const double scaleRotationRate = 1.0;
    msg.angular.z = scaleRotationRate * atan2(
        transformStamped.transform.translation.y,
        transformStamped.transform.translation.x);

    static const double scaleForwardSpeed = 0.5;
    msg.linear.x = scaleForwardSpeed * sqrt(
        pow(transformStamped.transform.translation.x, 2) +
        pow(transformStamped.transform.translation.y, 2));

    twist_pub_->publish(msg);

  }
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr twist_pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
  std::string target_frame;
  std::string source_frame;
};

int main(int argc, char const *argv[])
{
  // 2.初始化 ROS 客户端；
  rclcpp::init(argc,argv);
  // 4.调用 spin 函数，并传入对象指针；
  rclcpp::spin(std::make_shared<TurtleFrameListener>());
  // 5.释放资源。
  rclcpp::shutdown();
  return 0;
}

2.1.5 编写 launch 文件

        launch 目录下新建文件exer01_turtle_follow.launch.py，并编辑文件，输入如下内容：

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

def generate_launch_description():
    # 声明参数
    turtle1 = DeclareLaunchArgument(name="turtle1",default_value="turtle1")
    turtle2 = DeclareLaunchArgument(name="turtle2",default_value="turtle2")
    # 启动 turtlesim_node 节点
    turtlesim_node = Node(package="turtlesim", executable="turtlesim_node", name="t1")
    # 生成一只新乌龟
    spawn = Node(package="cpp05_exercise", executable="exer01_tf_spawn",
                name="spawn1",
                parameters=[{"turtle_name":LaunchConfiguration("turtle2")}]
    )
    # tf 广播
    tf_broadcaster1 = Node(package="cpp05_exercise",executable="exer02_tf_broadcaster",
                            name="tf_broadcaster1")
    tf_broadcaster2 = Node(package="cpp05_exercise",executable="exer02_tf_broadcaster",
                            name="tf_broadcaster1",
                            parameters=[{"turtle_name":LaunchConfiguration("turtle2")}])
    # tf 监听
    tf_listener = Node(package="cpp05_exercise",executable="exer03_tf_listener",
                            name="tf_listener",
                            parameters=[{"target_frame":LaunchConfiguration("turtle2"),"source_frame":LaunchConfiguration("turtle1")}]
                            )
    return LaunchDescription([turtle1,turtle2,turtlesim_node,spawn,tf_broadcaster1,tf_broadcaster2,tf_listener])

2.1.6 编辑配置文件

1.package.xml

在创建功能包时，所依赖的功能包已经自动配置了，配置内容如下：

<depend>rclcpp</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>geometry_msgs</depend>
<depend>turtlesim</depend>

2.CMakeLists.txt

CMakeLists.txt 中发布和订阅程序核心配置如下：

# 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(exer01_tf_spawn src/exer01_tf_spawn.cpp)
ament_target_dependencies(
  exer01_tf_spawn
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "turtlesim"
)

add_executable(exer02_tf_broadcaster src/exer02_tf_broadcaster.cpp)
ament_target_dependencies(
  exer02_tf_broadcaster
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "turtlesim"
)

add_executable(exer03_tf_listener src/exer03_tf_listener.cpp)
ament_target_dependencies(
  exer03_tf_listener
  "rclcpp"
  "tf2"
  "tf2_ros"
  "geometry_msgs"
  "turtlesim"
)

install(TARGETS 
  exer01_tf_spawn
  exer02_tf_broadcaster
  exer03_tf_listener
  DESTINATION lib/${PROJECT_NAME})

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)

2.1.7 编译执行

终端中进入当前工作空间，编译功能包：

colcon build --packages-select cpp05_exercise

当前工作空间下启动终端，输入如下命令运行launch文件：

. install/setup.bash 
ros2 launch cpp05_exercise exer01_turtle_follow.launch.py

再新建终端，启动 turtlesim 键盘控制节点：该终端下可以通过键盘控制 turtle1 运动，并且 turtle2 会跟随 turtle1 运动。最终的运行结果与演示案例类似。

ros2 run turtlesim turtle_teleop_key

最终效果展示。

2.2 Python实现流程

2.2.1 准备工作

1.新建功能包：

ros2 pkg create py05_exercise --build-type ament_python --dependencies rclpy tf_transformations tf2_ros geometry_msgs turtlesim

2.创建launch目录:

功能包py05_exercise下新建launch文件，并编辑配置文件。

功能包py05_exercise的 setup.py 文件中需要修改 data_files 属性，修改后的内容如下：

data_files=[
    ('share/ament_index/resource_index/packages',
        ['resource/' + package_name]),
    ('share/' + package_name, ['package.xml'],),
    ('share/' + package_name, glob("launch/*.launch.xml")),
    ('share/' + package_name, glob("launch/*.launch.py")),
    ('share/' + package_name, glob("launch/*.launch.yaml")),
],

2.2.2 编写生成新乌龟

        功能包 py05_exercise 的 py05_exercise 目录下，新建 Python 文件 exer01_tf_spawn_py.py，并编辑文件，输入如下内容：

"""  
  需求：编写客户端，发送请求生成一只新的乌龟。

  步骤：
    1.导包；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.声明并获取参数；
      3-2.创建客户端；
      3-3.等待服务连接；
      3-4.组织请求数据并发送；
    4.创建对象调用其功能,并处理响应；
    5.释放资源。  
"""
# 1.导包；
import rclpy
from rclpy.node import Node
from turtlesim.srv import Spawn

# 3.定义节点类；
class TurtleSpawnClient(Node):

    def __init__(self):
        super().__init__('turtle_spawn_client_py')

        # 3-1.声明并获取参数；
        self.x = self.declare_parameter("x",2.0)
        self.y = self.declare_parameter("y",2.0)
        self.theta = self.declare_parameter("theta",0.0)
        self.turtle_name = self.declare_parameter("turtle_name","turtle2")

        # 3-2.创建客户端；
        self.cli = self.create_client(Spawn, '/spawn')
        # 3-3.等待服务连接；
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('服务连接中，请稍候...')
        self.req = Spawn.Request()

    # 3-4.组织请求数据并发送；
    def send_request(self):
        self.req.x = self.get_parameter("x").get_parameter_value().double_value
        self.req.y = self.get_parameter("y").get_parameter_value().double_value
        self.req.theta = self.get_parameter("theta").get_parameter_value().double_value
        self.req.name = self.get_parameter("turtle_name").get_parameter_value().string_value
        self.future = self.cli.call_async(self.req)


def main():
    # 2.初始化 ROS2 客户端；
    rclpy.init()

    # 4.创建对象并调用其功能；
    client = TurtleSpawnClient()
    client.send_request()

    # 处理响应
    rclpy.spin_until_future_complete(client,client.future)
    try:
        response = client.future.result()
    except Exception as e:
        client.get_logger().info(
            '服务请求失败： %r' % e)
    else:
        if len(response.name) == 0:
            client.get_logger().info(
                '乌龟重名了，创建失败！')
        else:
            client.get_logger().info(
                '乌龟%s被创建' % response.name)

    # 5.释放资源。
    rclpy.shutdown()


if __name__ == '__main__':
    main()

2.2.3 编写坐标变换广播

        功能包 py05_exercise 的 py05_exercise 目录下，新建 Python 文件 exer02_tf_broadcaster_py.py，并编辑文件，输入如下内容：

"""  
  需求：发布乌龟坐标系到窗口坐标系的坐标变换。
  步骤：
    1.导包；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.声明并解析乌龟名称参数；
      3-2.创建动态坐标变换发布方；
      3-3.创建乌龟位姿订阅方；
      3-4.根据订阅到的乌龟位姿生成坐标帧并广播。
    4.调用 spin 函数，并传入对象；
    5.释放资源。

"""
# 1.导包；
from geometry_msgs.msg import TransformStamped
import rclpy
from rclpy.node import Node
from tf2_ros import TransformBroadcaster
import tf_transformations
from turtlesim.msg import Pose

# 3.定义节点类；
class TurtleFrameBroadcaster(Node):

    def __init__(self):
        super().__init__('turtle_frame_broadcaster_py')
        # 3-1.声明并解析乌龟名称参数；
        self.declare_parameter('turtle_name', 'turtle1')
        self.turtlename = self.get_parameter('turtle_name').get_parameter_value().string_value

        # 3-2.创建动态坐标变换发布方；
        self.br = TransformBroadcaster(self)

        # 3-3.创建乌龟位姿订阅方；
        self.subscription = self.create_subscription(
            Pose,
            self.turtlename+ '/pose',
            self.handle_turtle_pose,
            1)
        self.subscription

    def handle_turtle_pose(self, msg):
        # 3-4.根据订阅到的乌龟位姿生成坐标帧并广播。
        t = TransformStamped()

        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = self.turtlename

        t.transform.translation.x = msg.x
        t.transform.translation.y = msg.y
        t.transform.translation.z = 0.0

        q = tf_transformations.quaternion_from_euler(0, 0, msg.theta)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        self.br.sendTransform(t)


def main():
    # 2.初始化 ROS2 客户端；
    rclpy.init()
    # 4.调用 spin 函数，并传入对象；
    node = TurtleFrameBroadcaster()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    # 5.释放资源。
    rclpy.shutdown()

2.2.4 编写坐标变换监听

        功能包 py05_exercise 的 py05_exercise 目录下，新建 Python 文件 exer03_tf_listener_py.py，并编辑文件，输入如下内容：

"""  
  需求：广播的坐标系消息，并生成 turtle2 相对于 turtle1 的坐标系数据，
       并进一步生成控制 turtle2 运动的速度指令。
  步骤：
    1.导包；
    2.初始化 ROS2 客户端；
    3.定义节点类；
      3-1.声明并解析参数；
      3-2.创建tf缓存对象指针；
      3-3.创建tf监听器；
      3-4.按照条件查找符合条件的坐标系并生成变换后的坐标帧。
    4.调用 spin 函数，并传入对象；
    5.释放资源。

"""
# 1.导包；
import math
from geometry_msgs.msg import Twist
import rclpy
from rclpy.node import Node
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener

# 3.定义节点类；
class TurtleFrameListener(Node):

    def __init__(self):
        super().__init__('turtle_frame_listener_py')
        # 3-1.声明并解析参数；
        self.declare_parameter('target_frame', 'turtle2')
        self.declare_parameter('source_frame', 'turtle1')
        self.target_frame = self.get_parameter('target_frame').get_parameter_value().string_value
        self.source_frame = self.get_parameter('source_frame').get_parameter_value().string_value

        # 3-2.创建tf缓存对象指针；
        self.tf_buffer = Buffer()
        # 3-3.创建tf监听器；
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.publisher = self.create_publisher(Twist, self.target_frame + '/cmd_vel', 1)

        self.timer = self.create_timer(1.0, self.on_timer)

    def on_timer(self):
        # 3-4.按照条件查找符合条件的坐标系并生成变换后的坐标帧。
        try:
            now = rclpy.time.Time()
            trans = self.tf_buffer.lookup_transform(
                self.target_frame,
                self.source_frame,
                now)
        except TransformException as ex:
            self.get_logger().info(
                '%s 到 %s 坐标变换异常！' % (self.source_frame,self.target_frame))
            return

        # 3-5.生成 turtle2 的速度指令，并发布。
        msg = Twist()
        scale_rotation_rate = 1.0
        msg.angular.z = scale_rotation_rate * math.atan2(
            trans.transform.translation.y,
            trans.transform.translation.x)

        scale_forward_speed = 0.5
        msg.linear.x = scale_forward_speed * math.sqrt(
            trans.transform.translation.x ** 2 +
            trans.transform.translation.y ** 2)

        self.publisher.publish(msg)

def main():
    # 2.初始化 ROS2 客户端；
    rclpy.init()
    # 4.调用 spin 函数，并传入对象；
    node = TurtleFrameListener()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    # 5.释放资源。
    rclpy.shutdown()

2.2.5 编写 launch 文件

        launch 目录下新建文件 exer01_turtle_follow.launch.xml，并编辑文件，输入如下内容：

<launch>
    <!-- 乌龟准备 -->
    <node pkg="turtlesim" exec="turtlesim_node" name="t1" />
    <node pkg="py05_exercise" exec="exer01_tf_spawn_py" name="t2" />
    <!-- 发布坐标变换 -->
    <node pkg="py05_exercise" exec="exer02_tf_broadcaster_py" name="tf_broadcaster1_py">
    </node>
    <node pkg="py05_exercise" exec="exer02_tf_broadcaster_py" name="tf_broadcaster2_py">
        <param name="turtle_name" value="turtle2" />
    </node>
    <!-- 监听坐标变换 -->
    <node pkg="py05_exercise" exec="exer03_tf_listener_py" name="tf_listener_py">
        <param name="target_frame" value="turtle2" />
        <param name="source_frame" value="turtle1" />
    </node>
</launch>

2.2.6 编辑配置文件

1.package.xml

在创建功能包时，所依赖的功能包已经自动配置了，配置内容如下：

<depend>rclpy</depend>
<depend>tf_transformations</depend>
<depend>tf2_ros</depend>
<depend>geometry_msgs</depend>
<depend>turtlesim</depend>

2.setup.py

entry_points字段的console_scripts中添加如下内容：

entry_points={
    'console_scripts': [
        'exer01_tf_spawn_py = py05_exercise.exer01_tf_spawn_py:main',
        'exer02_tf_broadcaster_py = py05_exercise.exer02_tf_broadcaster_py:main',
        'exer03_tf_listener_py = py05_exercise.exer03_tf_listener_py:main'
    ],
},

2.2.7 编译执行

终端中进入当前工作空间，编译功能包：

colcon build --packages-select py05_exercise

当前工作空间下启动终端，输入如下命令运行launch文件：

. install/setup.bash 
ros2 launch py05_exercise exer01_turtle_follow.launch.xml

再新建终端，启动 turtlesim 键盘控制节点：

ros2 run turtlesim turtle_teleop_key

该终端下可以通过键盘控制 turtle1 运动，并且 turtle2 会跟随 turtle1 运动。最终的运行结果与C++实现类似。