【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 github /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++实现类似。