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

靜態(tài)TF廣播

我們說(shuō)TF的主要作用是對(duì)坐標(biāo)系進(jìn)行管理，那就管理一個(gè)試試唄？

坐標(biāo)變換中最為簡(jiǎn)單的應(yīng)該是相對(duì)位置不發(fā)生變化的情況，比如你家的房子在哪個(gè)位置，只要房子不拆，這個(gè)坐標(biāo)應(yīng)該就不會(huì)變化。

在機(jī)器人系統(tǒng)中也很常見(jiàn)，比如激光雷達(dá)和機(jī)器人底盤(pán)之間的位置關(guān)系，安裝好之后基本不會(huì)變化。

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

運(yùn)行效果

啟動(dòng)終端，運(yùn)行如下命令：

$ ros2 run learning_tf static_tf_broadcaster$ ros2 run tf2_tools view_frames

可以看到當(dāng)前系統(tǒng)中存在兩個(gè)坐標(biāo)系，一個(gè)是world，一個(gè)是house，兩者之間的相對(duì)位置不會(huì)發(fā)生改變，通過(guò)一個(gè)靜態(tài)的TF對(duì)象進(jìn)行維護(hù)。

代碼解析

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

learning_tf/static_tf_broadcaster.py

#!/usr/bin/env python3# -*- coding: utf-8 -*-"""@作者: 古月居(www.guyuehome.com)@說(shuō)明: ROS2 TF示例-廣播靜態(tài)的坐標(biāo)變換"""import rclpy                                                                 # ROS2 Python接口庫(kù)from rclpy.node import Node                                                  # ROS2 節(jié)點(diǎn)類from geometry_msgs.msg import TransformStamped                               # 坐標(biāo)變換消息import tf_transformations                                                    # TF坐標(biāo)變換庫(kù)from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster  # TF靜態(tài)坐標(biāo)系廣播器類class StaticTFBroadcaster(Node):    def __init__(self, name):        super().__init__(name)                                                  # ROS2節(jié)點(diǎn)父類初始化        self.tf_broadcaster = StaticTransformBroadcaster(self)                  # 創(chuàng)建一個(gè)TF廣播器對(duì)象        static_transformStamped = TransformStamped()                            # 創(chuàng)建一個(gè)坐標(biāo)變換的消息對(duì)象        static_transformStamped.header.stamp = self.get_clock().now().to_msg()  # 設(shè)置坐標(biāo)變換消息的時(shí)間戳        static_transformStamped.header.frame_id = 'world'                       # 設(shè)置一個(gè)坐標(biāo)變換的源坐標(biāo)系        static_transformStamped.child_frame_id  = 'house'                       # 設(shè)置一個(gè)坐標(biāo)變換的目標(biāo)坐標(biāo)系        static_transformStamped.transform.translation.x = 10.0                  # 設(shè)置坐標(biāo)變換中的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]                  # 設(shè)置坐標(biāo)變換中的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)坐標(biāo)變換，廣播后兩個(gè)坐標(biāo)系的位置關(guān)系保持不變def main(args=None):    rclpy.init(args=args)                                # ROS2 Python接口初始化    node = StaticTFBroadcaster("static_tf_broadcaster")  # 創(chuàng)建ROS2節(jié)點(diǎn)對(duì)象并進(jìn)行初始化    rclpy.spin(node)                                     # 循環(huán)等待ROS2退出    node.destroy_node()                                  # 銷毀節(jié)點(diǎn)對(duì)象    rclpy.shutdown()

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

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

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

TF監(jiān)聽(tīng)

我們?cè)賮?lái)學(xué)習(xí)下如何查詢兩個(gè)坐標(biāo)系之間的位置關(guān)系。

運(yùn)行效果

啟動(dòng)一個(gè)終端，運(yùn)行如下節(jié)點(diǎn)，就可以在終端中看到周期顯示的坐標(biāo)關(guān)系了。

$ ros2 run learning_tf tf_listener

代碼解析

這個(gè)節(jié)點(diǎn)中是如何查詢坐標(biāo)關(guān)系的，我們來(lái)看下代碼

learning_tf/tf_listener.py

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

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

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

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