靜態TF廣播

我們說TF的主要作用是對坐標系進行管理，那就管理一個試試唄？

坐標變換中最為簡單的應該是相對位置不發生變化的情況，比如你家的房子在哪個位置，只要房子不拆，這個坐標應該就不會變化。

在機器人系統中也很常見，比如激光雷達和機器人底盤之間的位置關系，安裝好之后基本不會變化。

在TF中，這種情況也稱之為靜態TF變換，我們來看看在程序中該如何實現？

運行效果

啟動終端，運行如下命令：

$ ros2 run learning_tf static_tf_broadcaster$ ros2 run tf2_tools view_frames

可以看到當前系統中存在兩個坐標系，一個是world，一個是house，兩者之間的相對位置不會發生改變，通過一個靜態的TF對象進行維護。

代碼解析

來看下在代碼中是如何創建坐標系并且發布靜態變換的。

learning_tf/static_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.static_transform_broadcaster import StaticTransformBroadcaster  # TF靜態坐標系廣播器類class StaticTFBroadcaster(Node):    def __init__(self, name):        super().__init__(name)                                                  # ROS2節點父類初始化        self.tf_broadcaster = StaticTransformBroadcaster(self)                  # 創建一個TF廣播器對象        static_transformStamped = TransformStamped()                            # 創建一個坐標變換的消息對象        static_transformStamped.header.stamp = self.get_clock().now().to_msg()  # 設置坐標變換消息的時間戳        static_transformStamped.header.frame_id = 'world'                       # 設置一個坐標變換的源坐標系        static_transformStamped.child_frame_id  = 'house'                       # 設置一個坐標變換的目標坐標系        static_transformStamped.transform.translation.x = 10.0                  # 設置坐標變換中的X、Y、Z向的平移        static_transformStamped.transform.translation.y = 5.0                            static_transformStamped.transform.translation.z = 0.0        quat = tf_transformations.quaternion_from_euler(0.0, 0.0, 0.0)          # 將歐拉角轉換為四元數（roll, pitch, yaw）        static_transformStamped.transform.rotation.x = quat[0]                  # 設置坐標變換中的X、Y、Z向的旋轉（四元數）        static_transformStamped.transform.rotation.y = quat[1]        static_transformStamped.transform.rotation.z = quat[2]        static_transformStamped.transform.rotation.w = quat[3]        self.tf_broadcaster.sendTransform(static_transformStamped)              # 廣播靜態坐標變換，廣播后兩個坐標系的位置關系保持不變def main(args=None):    rclpy.init(args=args)                                # ROS2 Python接口初始化    node = StaticTFBroadcaster("static_tf_broadcaster")  # 創建ROS2節點對象并進行初始化    rclpy.spin(node)                                     # 循環等待ROS2退出    node.destroy_node()                                  # 銷毀節點對象    rclpy.shutdown()

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

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

經過這段代碼，兩個坐標系的變化是描述清楚了，到了使用的時候，我們又該如何查詢呢？

TF監聽

我們再來學習下如何查詢兩個坐標系之間的位置關系。

運行效果

啟動一個終端，運行如下節點，就可以在終端中看到周期顯示的坐標關系了。

$ ros2 run learning_tf tf_listener

代碼解析

這個節點中是如何查詢坐標關系的，我們來看下代碼

learning_tf/tf_listener.py

#!/usr/bin/env python3# -*- coding: utf-8 -*-"""@作者: 古月居(www.guyuehome.com)@說明: ROS2 TF示例-監聽某兩個坐標系之間的變換"""import 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  # 監聽坐標變換的監聽器類class TFListener(Node):    def __init__(self, name):        super().__init__(name)                                      # ROS2節點父類初始化        self.declare_parameter('source_frame', 'world')             # 創建一個源坐標系名的參數        self.source_frame = self.get_parameter(                     # 優先使用外部設置的參數值，否則用默認值            'source_frame').get_parameter_value().string_value        self.declare_parameter('target_frame', 'house')             # 創建一個目標坐標系名的參數        self.target_frame = self.get_parameter(                     # 優先使用外部設置的參數值，否則用默認值            'target_frame').get_parameter_value().string_value        self.tf_buffer = Buffer()                                   # 創建保存坐標變換信息的緩沖區        self.tf_listener = TransformListener(self.tf_buffer, self)  # 創建坐標變換的監聽器        self.timer = self.create_timer(1.0, self.on_timer)          # 創建一個固定周期的定時器，處理坐標信息    def on_timer(self):        try:            now = rclpy.time.Time()                                 # 獲取ROS系統的當前時間            trans = self.tf_buffer.lookup_transform(                # 監聽當前時刻源坐標系到目標坐標系的坐標變換                self.target_frame,                self.source_frame,                now)        except TransformException as ex:                            # 如果坐標變換獲取失敗，進入異常報告            self.get_logger().info(                f'Could not transform {self.target_frame} to {self.source_frame}: {ex}')            return        pos  = trans.transform.translation                          # 獲取位置信息        quat = trans.transform.rotation                             # 獲取姿態信息（四元數）        euler = tf_transformations.euler_from_quaternion([quat.x, quat.y, quat.z, quat.w])        self.get_logger().info('Get %s -- > %s transform: [%f, %f, %f] [%f, %f, %f]'           % (self.source_frame, self.target_frame, pos.x, pos.y, pos.z, euler[0], euler[1], euler[2]))def main(args=None):    rclpy.init(args=args)                       # ROS2 Python接口初始化    node = TFListener("tf_listener")            # 創建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',            'tf_listener = learning_tf.tf_listener:main',        ],    },

好啦，大家現在對TF的基本使用有所了解了。我們繼續挑戰兩只海龜跟隨的案例。