Launch文件解析

先來看下剛才運(yùn)行的launch文件，里邊啟動(dòng)了四個(gè)節(jié)點(diǎn)，分別是：

• 小海龜仿真器
• 海龜1的坐標(biāo)系廣播
• 海龜2的坐標(biāo)系廣播
• 海龜跟隨控制

其中，兩個(gè)坐標(biāo)系的廣播復(fù)用了turtle_tf_broadcaster節(jié)點(diǎn)，通過傳入的參數(shù)名修改維護(hù)的坐標(biāo)系名稱。

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')}            ]        ),     ])

坐標(biāo)系動(dòng)態(tài)廣播

海龜1和海龜2在world坐標(biāo)系下的坐標(biāo)變換，在turtle_tf_broadcaster節(jié)點(diǎn)中實(shí)現(xiàn)，除了海龜坐標(biāo)系的名字不同之外，針對兩個(gè)海龜?shù)墓δ苁且粯拥摹?/p>

learning_tf/turtle_tf_broadcaster.py

#!/usr/bin/env python3# -*- coding: utf-8 -*-"""@作者: 古月居(www.guyuehome.com)@說明: ROS2 TF示例-廣播動(dòng)態(tài)的坐標(biāo)變換"""import rclpy                                       # ROS2 Python接口庫from rclpy.node import Node                        # ROS2 節(jié)點(diǎn)類from geometry_msgs.msg import TransformStamped     # 坐標(biāo)變換消息import tf_transformations                          # TF坐標(biāo)變換庫from tf2_ros import TransformBroadcaster           # TF坐標(biāo)變換廣播器from turtlesim.msg import Pose                     # turtlesim小海龜位置消息class TurtleTFBroadcaster(Node):    def __init__(self, name):        super().__init__(name)                                # ROS2節(jié)點(diǎn)父類初始化        self.declare_parameter('turtlename', 'turtle')        # 創(chuàng)建一個(gè)海龜名稱的參數(shù)        self.turtlename = self.get_parameter(                 # 優(yōu)先使用外部設(shè)置的參數(shù)值，否則用默認(rèn)值            'turtlename').get_parameter_value().string_value        self.tf_broadcaster = TransformBroadcaster(self)      # 創(chuàng)建一個(gè)TF坐標(biāo)變換的廣播對象并初始化        self.subscription = self.create_subscription(         # 創(chuàng)建一個(gè)訂閱者，訂閱海龜?shù)奈恢孟?           Pose,            f'/{self.turtlename}/pose',                       # 使用參數(shù)中獲取到的海龜名稱            self.turtle_pose_callback, 1)    def turtle_pose_callback(self, msg):                              # 創(chuàng)建一個(gè)處理海龜位置消息的回調(diào)函數(shù)，將位置消息轉(zhuǎn)變成坐標(biāo)變換        transform = TransformStamped()                                # 創(chuàng)建一個(gè)坐標(biāo)變換的消息對象        transform.header.stamp = self.get_clock().now().to_msg()      # 設(shè)置坐標(biāo)變換消息的時(shí)間戳        transform.header.frame_id = 'world'                           # 設(shè)置一個(gè)坐標(biāo)變換的源坐標(biāo)系        transform.child_frame_id = self.turtlename                    # 設(shè)置一個(gè)坐標(biāo)變換的目標(biāo)坐標(biāo)系        transform.transform.translation.x = msg.x                     # 設(shè)置坐標(biāo)變換中的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) # 將歐拉角轉(zhuǎn)換為四元數(shù)（roll, pitch, yaw）        transform.transform.rotation.x = q[0]                         # 設(shè)置坐標(biāo)變換中的X、Y、Z向的旋轉(zhuǎn)（四元數(shù)）        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)     # 廣播坐標(biāo)變換，海龜位置變化后，將及時(shí)更新坐標(biāo)變換信息def main(args=None):    rclpy.init(args=args)                                # ROS2 Python接口初始化    node = TurtleTFBroadcaster("turtle_tf_broadcaster")  # 創(chuàng)建ROS2節(jié)點(diǎn)對象并進(jìn)行初始化    rclpy.spin(node)                                     # 循環(huán)等待ROS2退出    node.destroy_node()                                  # 銷毀節(jié)點(diǎn)對象    rclpy.shutdown()                                     # 關(guān)閉ROS2 Python接口

完成代碼的編寫后需要設(shè)置功能包的編譯選項(xiàng)，讓系統(tǒng)知道Python程序的入口，打開功能包的setup.py文件，加入如下入口點(diǎn)的配置：

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',        ],    },

海龜跟隨

坐標(biāo)系都正常廣播了，接下來我們就可以訂閱兩只海龜?shù)奈恢藐P(guān)系，并且變換成速度指令進(jìn)行控制啦。

learning_tf/turtle_following.py

#!/usr/bin/env python3# -*- coding: utf-8 -*-"""@作者: 古月居(www.guyuehome.com)@說明: ROS2 TF示例-通過坐標(biāo)變化實(shí)現(xiàn)海龜跟隨功能"""import mathimport rclpy                                              # ROS2 Python接口庫from rclpy.node import Node                               # ROS2 節(jié)點(diǎn)類import tf_transformations                                 # TF坐標(biāo)變換庫from tf2_ros import TransformException                    # TF左邊變換的異常類from tf2_ros.buffer import Buffer                         # 存儲(chǔ)坐標(biāo)變換信息的緩沖類from tf2_ros.transform_listener import TransformListener  # 監(jiān)聽坐標(biāo)變換的監(jiān)聽器類from geometry_msgs.msg import Twist                       # ROS2 速度控制消息from turtlesim.srv import Spawn                           # 海龜生成的服務(wù)接口class TurtleFollowing(Node):    def __init__(self, name):        super().__init__(name)                                      # ROS2節(jié)點(diǎn)父類初始化        self.declare_parameter('source_frame', 'turtle1')           # 創(chuàng)建一個(gè)源坐標(biāo)系名的參數(shù)        self.source_frame = self.get_parameter(                     # 優(yōu)先使用外部設(shè)置的參數(shù)值，否則用默認(rèn)值            'source_frame').get_parameter_value().string_value        self.tf_buffer = Buffer()                                   # 創(chuàng)建保存坐標(biāo)變換信息的緩沖區(qū)        self.tf_listener = TransformListener(self.tf_buffer, self)  # 創(chuàng)建坐標(biāo)變換的監(jiān)聽器        self.spawner = self.create_client(Spawn, 'spawn')           # 創(chuàng)建一個(gè)請求產(chǎn)生海龜?shù)目蛻舳?       self.turtle_spawning_service_ready = False                  # 是否已經(jīng)請求海龜生成服務(wù)的標(biāo)志位        self.turtle_spawned = False                                 # 海龜是否產(chǎn)生成功的標(biāo)志位        self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1) # 創(chuàng)建跟隨運(yùn)動(dòng)海龜?shù)乃俣仍掝}        self.timer = self.create_timer(1.0, self.on_timer)         # 創(chuàng)建一個(gè)固定周期的定時(shí)器，控制跟隨海龜?shù)倪\(yùn)動(dòng)    def on_timer(self):        from_frame_rel = self.source_frame                         # 源坐標(biāo)系        to_frame_rel   = 'turtle2'                                 # 目標(biāo)坐標(biāo)系        if self.turtle_spawning_service_ready:                     # 如果已經(jīng)請求海龜生成服務(wù)            if self.turtle_spawned:                                # 如果跟隨海龜已經(jīng)生成                try:                    now = rclpy.time.Time()                        # 獲取ROS系統(tǒng)的當(dāng)前時(shí)間                    trans = self.tf_buffer.lookup_transform(       # 監(jiān)聽當(dāng)前時(shí)刻源坐標(biāo)系到目標(biāo)坐標(biāo)系的坐標(biāo)變換                        to_frame_rel,                        from_frame_rel,                        now)                except TransformException as ex:                   # 如果坐標(biāo)變換獲取失敗，進(jìn)入異常報(bào)告                    self.get_logger().info(                        f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')                    return                msg = Twist()                                      # 創(chuàng)建速度控制消息                scale_rotation_rate = 1.0                          # 根據(jù)海龜角度，計(jì)算角速度                msg.angular.z = scale_rotation_rate * math.atan2(                    trans.transform.translation.y,                    trans.transform.translation.x)                scale_forward_speed = 0.5                          # 根據(jù)海龜距離，計(jì)算線速度                msg.linear.x = scale_forward_speed * math.sqrt(                    trans.transform.translation.x ** 2 +                    trans.transform.translation.y ** 2)                self.publisher.publish(msg)                        # 發(fā)布速度指令，海龜跟隨運(yùn)動(dòng)            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:                                                      # 如果沒有請求海龜生成服務(wù)            if self.spawner.service_is_ready():                    # 如果海龜生成服務(wù)器已經(jīng)準(zhǔn)備就緒                request = Spawn.Request()                          # 創(chuàng)建一個(gè)請求的數(shù)據(jù)                request.name = 'turtle2'                           # 設(shè)置請求數(shù)據(jù)的內(nèi)容，包括海龜名、xy位置、姿態(tài)                request.x = float(4)                request.y = float(2)                request.theta = float(0)                self.result = self.spawner.call_async(request)     # 發(fā)送服務(wù)請求                self.turtle_spawning_service_ready = True          # 設(shè)置標(biāo)志位，表示已經(jīng)發(fā)送請求            else:                self.get_logger().info('Service is not ready')     # 海龜生成服務(wù)器還沒準(zhǔn)備就緒的提示def main(args=None):    rclpy.init(args=args)                       # ROS2 Python接口初始化    node = TurtleFollowing("turtle_following")  # 創(chuàng)建ROS2節(jié)點(diǎn)對象并進(jìn)行初始化    rclpy.spin(node)                            # 循環(huán)等待ROS2退出    node.destroy_node()                         # 銷毀節(jié)點(diǎn)對象    rclpy.shutdown()                            # 關(guān)閉ROS2 Python接口

完成代碼的編寫后需要設(shè)置功能包的編譯選項(xiàng)，讓系統(tǒng)知道Python程序的入口，打開功能包的setup.py文件，加入如下入口點(diǎn)的配置：

entry_points={        'console_scripts': [            'static_tf_broadcast