如何對機器人靜態(tài)TF廣播進行管理

靜態(tài)TF廣播

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

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

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

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

運行效果

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

$ ros2 run learning_tf static_tf_broadcaster$ ros2 run tf2_tools view_frames

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

代碼解析

來看下在代碼中是如何創(chuàng)建坐標系并且發(fā)布靜態(tài)變換的。

learning_tf/static_tf_broadcaster.py

#!/usr/bin/env python3# -*- coding: utf-8 -*-"""@作者: 古月居(www.guyuehome.com)@說明: ROS2 TF示例-廣播靜態(tài)的坐標變換"""import rclpy                                                                 # ROS2 Python接口庫from rclpy.node import Node                                                  # ROS2 節(jié)點類from geometry_msgs.msg import TransformStamped                               # 坐標變換消息import tf_transformations                                                    # TF坐標變換庫from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster  # TF靜態(tài)坐標系廣播器類class StaticTFBroadcaster(Node):    def __init__(self, name):        super().__init__(name)                                                  # ROS2節(jié)點父類初始化        self.tf_broadcaster = StaticTransformBroadcaster(self)                  # 創(chuàng)建一個TF廣播器對象        static_transformStamped = TransformStamped()                            # 創(chuàng)建一個坐標變換的消息對象        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)          # 將歐拉角轉(zhuǎn)換為四元數(shù)（roll, pitch, yaw）        static_transformStamped.transform.rotation.x = quat[0]                  # 設置坐標變換中的X、Y、Z向的旋轉(zhuǎn)（四元數(shù)）        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)              # 廣播靜態(tài)坐標變換，廣播后兩個坐標系的位置關系保持不變def main(args=None):    rclpy.init(args=args)                                # ROS2 Python接口初始化    node = StaticTFBroadcaster("static_tf_broadcaster")  # 創(chuàng)建ROS2節(jié)點對象并進行初始化    rclpy.spin(node)                                     # 循環(huán)等待ROS2退出    node.destroy_node()                                  # 銷毀節(jié)點對象    rclpy.shutdown()

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

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

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

TF監(jiān)聽

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

運行效果

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

$ ros2 run learning_tf tf_listener

代碼解析

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

learning_tf/tf_listener.py

#!/usr/bin/env python3# -*- coding: utf-8 -*-"""@作者: 古月居(www.guyuehome.com)@說明: ROS2 TF示例-監(jiān)聽某兩個坐標系之間的變換"""import rclpy                                              # ROS2 Python接口庫from rclpy.node import Node                               # ROS2 節(jié)點類import tf_transformations                                 # TF坐標變換庫from tf2_ros import TransformException                    # TF左邊變換的異常類from tf2_ros.buffer import Buffer                         # 存儲坐標變換信息的緩沖類from tf2_ros.transform_listener import TransformListener  # 監(jiān)聽坐標變換的監(jiān)聽器類class TFListener(Node):    def __init__(self, name):        super().__init__(name)                                      # ROS2節(jié)點父類初始化        self.declare_parameter('source_frame', 'world')             # 創(chuàng)建一個源坐標系名的參數(shù)        self.source_frame = self.get_parameter(                     # 優(yōu)先使用外部設置的參數(shù)值，否則用默認值            'source_frame').get_parameter_value().string_value        self.declare_parameter('target_frame', 'house')             # 創(chuàng)建一個目標坐標系名的參數(shù)        self.target_frame = self.get_parameter(                     # 優(yōu)先使用外部設置的參數(shù)值，否則用默認值            'target_frame').get_parameter_value().string_value        self.tf_buffer = Buffer()                                   # 創(chuàng)建保存坐標變換信息的緩沖區(qū)        self.tf_listener = TransformListener(self.tf_buffer, self)  # 創(chuàng)建坐標變換的監(jiān)聽器        self.timer = self.create_timer(1.0, self.on_timer)          # 創(chuàng)建一個固定周期的定時器，處理坐標信息    def on_timer(self):        try:            now = rclpy.time.Time()                                 # 獲取ROS系統(tǒng)的當前時間            trans = self.tf_buffer.lookup_transform(                # 監(jiān)聽當前時刻源坐標系到目標坐標系的坐標變換                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                             # 獲取姿態(tài)信息（四元數(shù)）        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")            # 創(chuàng)建ROS2節(jié)點對象并進行初始化    rclpy.spin(node)                            # 循環(huán)等待ROS2退出    node.destroy_node()                         # 銷毀節(jié)點對象    rclpy.shutdown()                            # 關閉ROS2 Python接口

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

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

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