Launch文件解析

先來看下剛才運行的launch文件，里邊啟動了四個節點，分別是：

• 小海龜仿真器
• 海龜1的坐標系廣播
• 海龜2的坐標系廣播
• 海龜跟隨控制

其中，兩個坐標系的廣播復用了turtle_tf_broadcaster節點，通過傳入的參數名修改維護的坐標系名稱。

learning_tf/launch/turtle_following_demo.launch.py

from launch import LaunchDescriptionfrom launch.actions import DeclareLaunchArgumentfrom launch.substitutions import LaunchConfigurationfrom launch_ros.actions import Nodedef generate_launch_description():    return LaunchDescription([        Node(            package='turtlesim',            executable='turtlesim_node',            name='sim'        ),        Node(            package='learning_tf',            executable='turtle_tf_broadcaster',            name='broadcaster1',            parameters=[                {'turtlename': 'turtle1'}            ]        ),        DeclareLaunchArgument(            'target_frame', default_value='turtle1',            description='Target frame name.'        ),        Node(            package='learning_tf',            executable='turtle_tf_broadcaster',            name='broadcaster2',            parameters=[                {'turtlename': 'turtle2'}            ]        ),        Node(            package='learning_tf',            executable='turtle_following',            name='listener',            parameters=[                {'target_frame': LaunchConfiguration('target_frame')}            ]        ),     ])

坐標系動態廣播

海龜1和海龜2在world坐標系下的坐標變換，在turtle_tf_broadcaster節點中實現，除了海龜坐標系的名字不同之外，針對兩個海龜的功能是一樣的。

learning_tf/turtle_tf_broadcaster.py

#!/usr/bin/env python3# -*- coding: utf-8 -*-"""@作者: 古月居(www.guyuehome.com)@說明: ROS2 TF示例-廣播動態的坐標變換"""import rclpy                                       # ROS2 Python接口庫from rclpy.node import Node                        # ROS2 節點類from geometry_msgs.msg import TransformStamped     # 坐標變換消息import tf_transformations                          # TF坐標變換庫from tf2_ros import TransformBroadcaster           # TF坐標變換廣播器from turtlesim.msg import Pose                     # turtlesim小海龜位置消息class TurtleTFBroadcaster(Node):    def __init__(self, name):        super().__init__(name)                                # ROS2節點父類初始化        self.declare_parameter('turtlename', 'turtle')        # 創建一個海龜名稱的參數        self.turtlename = self.get_parameter(                 # 優先使用外部設置的參數值，否則用默認值            'turtlename').get_parameter_value().string_value        self.tf_broadcaster = TransformBroadcaster(self)      # 創建一個TF坐標變換的廣播對象并初始化        self.subscription = self.create_subscription(         # 創建一個訂閱者，訂閱海龜的位置消息            Pose,            f'/{self.turtlename}/pose',                       # 使用參數中獲取到的海龜名稱            self.turtle_pose_callback, 1)    def turtle_pose_callback(self, msg):                              # 創建一個處理海龜位置消息的回調函數，將位置消息轉變成坐標變換        transform = TransformStamped()                                # 創建一個坐標變換的消息對象        transform.header.stamp = self.get_clock().now().to_msg()      # 設置坐標變換消息的時間戳        transform.header.frame_id = 'world'                           # 設置一個坐標變換的源坐標系        transform.child_frame_id = self.turtlename                    # 設置一個坐標變換的目標坐標系        transform.transform.translation.x = msg.x                     # 設置坐標變換中的X、Y、Z向的平移        transform.transform.translation.y = msg.y        transform.transform.translation.z = 0.0        q = tf_transformations.quaternion_from_euler(0, 0, msg.theta) # 將歐拉角轉換為四元數（roll, pitch, yaw）        transform.transform.rotation.x = q[0]                         # 設置坐標變換中的X、Y、Z向的旋轉（四元數）        transform.transform.rotation.y = q[1]        transform.transform.rotation.z = q[2]        transform.transform.rotation.w = q[3]        # Send the transformation        self.tf_broadcaster.sendTransform(transform)     # 廣播坐標變換，海龜位置變化后，將及時更新坐標變換信息def main(args=None):    rclpy.init(args=args)                                # ROS2 Python接口初始化    node = TurtleTFBroadcaster("turtle_tf_broadcaster")  # 創建ROS2節點對象并進行初始化    rclpy.spin(node)                                     # 循環等待ROS2退出    node.destroy_node()                                  # 銷毀節點對象    rclpy.shutdown()                                     # 關閉ROS2 Python接口

完成代碼的編寫后需要設置功能包的編譯選項，讓系統知道Python程序的入口，打開功能包的setup.py文件，加入如下入口點的配置：

entry_points={        'console_scripts': [            'static_tf_broadcaster = learning_tf.static_tf_broadcaster:main',            'turtle_tf_broadcaster = learning_tf.turtle_tf_broadcaster:main',            'tf_listener = learning_tf.tf_listener:main',        ],    },

海龜跟隨

坐標系都正常廣播了，接下來我們就可以訂閱兩只海龜的位置關系，并且變換成速度指令進行控制啦。

learning_tf/turtle_following.py

#!/usr/bin/env python3# -*- coding: utf-8 -*-"""@作者: 古月居(www.guyuehome.com)@說明: ROS2 TF示例-通過坐標變化實現海龜跟隨功能"""import mathimport rclpy                                              # ROS2 Python接口庫from rclpy.node import Node                               # ROS2 節點類import tf_transformations                                 # TF坐標變換庫from tf2_ros import TransformException                    # TF左邊變換的異常類from tf2_ros.buffer import Buffer                         # 存儲坐標變換信息的緩沖類from tf2_ros.transform_listener import TransformListener  # 監聽坐標變換的監聽器類from geometry_msgs.msg import Twist                       # ROS2 速度控制消息from turtlesim.srv import Spawn                           # 海龜生成的服務接口class TurtleFollowing(Node):    def __init__(self, name):        super().__init__(name)                                      # ROS2節點父類初始化        self.declare_parameter('source_frame', 'turtle1')           # 創建一個源坐標系名的參數        self.source_frame = self.get_parameter(                     # 優先使用外部設置的參數值，否則用默認值            'source_frame').get_parameter_value().string_value        self.tf_buffer = Buffer()                                   # 創建保存坐標變換信息的緩沖區        self.tf_listener = TransformListener(self.tf_buffer, self)  # 創建坐標變換的監聽器        self.spawner = self.create_client(Spawn, 'spawn')           # 創建一個請求產生海龜的客戶端        self.turtle_spawning_service_ready = False                  # 是否已經請求海龜生成服務的標志位        self.turtle_spawned = False                                 # 海龜是否產生成功的標志位        self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1) # 創建跟隨運動海龜的速度話題        self.timer = self.create_timer(1.0, self.on_timer)         # 創建一個固定周期的定時器，控制跟隨海龜的運動    def on_timer(self):        from_frame_rel = self.source_frame                         # 源坐標系        to_frame_rel   = 'turtle2'                                 # 目標坐標系        if self.turtle_spawning_service_ready:                     # 如果已經請求海龜生成服務            if self.turtle_spawned:                                # 如果跟隨海龜已經生成                try:                    now = rclpy.time.Time()                        # 獲取ROS系統的當前時間                    trans = self.tf_buffer.lookup_transform(       # 監聽當前時刻源坐標系到目標坐標系的坐標變換                        to_frame_rel,                        from_frame_rel,                        now)                except TransformException as ex:                   # 如果坐標變換獲取失敗，進入異常報告                    self.get_logger().info(                        f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')                    return                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)                        # 發布速度指令，海龜跟隨運動            else:                                                  # 如果跟隨海龜沒有生成                if self.result.done():                             # 查看海龜是否生成                    self.get_logger().info(                        f'Successfully spawned {self.result.result().name}')                    self.turtle_spawned = True                                     else:                                              # 依然沒有生成跟隨海龜                    self.get_logger().info('Spawn is not finished')        else:                                                      # 如果沒有請求海龜生成服務            if self.spawner.service_is_ready():                    # 如果海龜生成服務器已經準備就緒                request = Spawn.Request()                          # 創建一個請求的數據                request.name = 'turtle2'                           # 設置請求數據的內容，包括海龜名、xy位置、姿態                request.x = float(4)                request.y = float(2)                request.theta = float(0)                self.result = self.spawner.call_async(request)     # 發送服務請求                self.turtle_spawning_service_ready = True          # 設置標志位，表示已經發送請求            else:                self.get_logger().info('Service is not ready')     # 海龜生成服務器還沒準備就緒的提示def main(args=None):    rclpy.init(args=args)                       # ROS2 Python接口初始化    node = TurtleFollowing("turtle_following")  # 創建ROS2節點對象并進行初始化    rclpy.spin(node)                            # 循環等待ROS2退出    node.destroy_node()                         # 銷毀節點對象    rclpy.shutdown()                            # 關閉ROS2 Python接口

完成代碼的編寫后需要設置功能包的編譯選項，讓系統知道Python程序的入口，打開功能包的setup.py文件，加入如下入口點的配置：

entry_points={        'console_scripts': [            'static_tf_broadcast