【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 headerbuiltin_interfaces/Time stamp     # 时间戳int32 secuint32 nanosecstring frame_id                   # 父级坐标系string child_frame_id                 # 子级坐标系Transform transform                   # 子级坐标系相对于父级坐标系的位姿Vector3 translation               # 三维偏移量float64 xfloat64 yfloat64 zQuaternion rotation               # 四元数float64 x 0float64 y 0float64 z 0float64 w 1

        第二种：geometry_msgs/msg/PointStamped

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

ros2 interface show geometry_msgs/msg/PointStamped

接口定义解释：

std_msgs/Header headerbuiltin_interfaces/Time stamp    # 时间戳int32 secuint32 nanosecstring frame_id                  # 参考系
Point point                          # 三维坐标float64 xfloat64 yfloat64 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 launchDESTINATION 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 LaunchConfigurationdef 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_spawnexer02_tf_broadcasterexer03_tf_listenerDESTINATION lib/${PROJECT_NAME})install(DIRECTORY launchDESTINATION 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_valueself.req.y = self.get_parameter("y").get_parameter_value().double_valueself.req.theta = self.get_parameter("theta").get_parameter_value().double_valueself.req.name = self.get_parameter("turtle_name").get_parameter_value().string_valueself.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.subscriptiondef 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.turtlenamet.transform.translation.x = msg.xt.transform.translation.y = msg.yt.transform.translation.z = 0.0q = 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_valueself.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.0msg.angular.z = scale_rotation_rate * math.atan2(trans.transform.translation.y,trans.transform.translation.x)scale_forward_speed = 0.5msg.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++实现类似。