ROS2 TF机器人坐标系管理神器

TF：机器人坐标系管理神器

1.机器人中的坐标系

机器人安装位置基坐标系（Base Frame）,机器人安装位置在外部环境下的参考系叫做世界坐标系（World Frame）,机器人末端夹子的位置叫做工具坐标系，外部被操作物体的位置叫做工件坐标系，机械臂在抓取物体的过程中，这些坐标系之间的关系也会跟随变化。

在移动机器人系统中，坐标系一样至关重要，比如一个移动机器人的中心点是基坐标系Base Link，雷达所在的位置叫做雷达坐标系laser link，机器人要移动，里程计会累积位置，这个位置的参考系叫做里程计坐标系odom，里程计又会有累积误差和漂移，绝对位置的参考系叫做地图坐标系map。

一层一层坐标系之间关系十分复杂，有一些是相对固定的，也有一些是不断变化的，良好的坐标系管理系统显得格外重要

坐标系变化可以分为平移和旋转两部分，通过四乘四的矩阵进行描述，在空间中画出坐标系，两者之间的变换关系，其实就是向量的数学描述

ROS中TF功能的底层原理，就是对这些数学变换进行了封装

2.TF命令行操作

• 小乌龟跟随例程
  需要先安装相应的功能包，然后就可以通过launch文件启动，之后我们可以控制其中的一只小海龟，另外一只小海龟会自动跟随运动
  安装功能包

$ sudo apt install ros-humble-turtle-tf2-py ros-humble-tf2-tools
$ sudo pip3 install transforms3d

运行launch文件

$ ros2 launch turtle_tf2_py turtle_tf2_demo.launch.py
$ ros2 run turtlesim turtle_teleop_key

当我们控制一只海龟运动时，另外一只海龟也会跟随运动

查看TF树
可以借助工具来查看两只小海龟有哪些坐标系

$ ros2 run tf2_tools view_frames 

默认在当前终端路径下生成一个frames.pdf文件，打开之后可以看到系统中各个坐标系的关系 world坐标系在左下角，turtle1和turtle2分别在两只小海龟的中心点，随着运动，turtle1与turtle2相对于world坐标系会随之变化

查询坐标变换信息
通过tf2_echo这个工具可以查看两个坐标系之间的具体关系，而不是仅仅看到坐标系的结构
查看turtle2相对于turtle1的坐标变换

$ ros2 run tf2_ros tf2_echo turtle2 turtle1

旋转有三种描述方式：1.四元素 2.欧拉角的弧度值 3.欧拉角的角度值

坐标系可视化
借用rviz可视化工具来进一步明显的查看两只小乌龟的坐标变换

$ ros2 run rviz2 rviz2 -d $(ros2 pkg prefix --share turtle_tf2_py)/rviz/turtle_rviz.rviz

3.静态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

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广播器对象，将TF消息广播出去
 
        static_transformStamped = TransformStamped()                            # 创建一个坐标变换的消息对象
        static_transformStamped.header.stamp = self.get_clock().now().to_msg()  # 设置坐标变换消息的时间戳，告诉ROS2系统当前变换关系是在哪一个时间点的信息
        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监听

4.TF监听
查询两个坐标系之间的位置关系
运行一个广播器，再运行一个监听器去监听广播

$ ros2 run learning_tf tf_listener

可以看到周期性的显示坐标关系

代码解析
learning_tf/tf_listener.py

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')             # 创建一个源坐标系名的参数 后面可以通过修改参数将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(                # 监听当前时刻源坐标系到目标坐标系的坐标变换 lookup_transform函数式查看两个坐标系的变换 trans就是坐标变换的数值
                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',
        ],
    },

5.海龟跟随功能解析
还是之前小海龟跟随的示例，自己通过代码进行实现

$ ros2 launch learning_tf turtle_following_demo.launch.py
$ ros2 run turtlesim turtle_teleop_key

原理解析

在两只海龟的仿真器中，定义三个坐标系，比如仿真器的全局参考系叫做world，turtle1和turtle2坐标系在两只海龟的中心点，这样，turtle1和world坐标系的相对位置，就可以表示海龟1的位置，海龟2也同理。

要实现海龟2向海龟1运动，我们在两者中间做一个连线，再加一个箭头，我们说坐标变换的描述方法就是向量，所以在这个跟随例程中，用TF就可以很好的解决。

向量的长度表示距离，方向表示角度，有了距离和角度，随便设置一个时间，就可以计算得到速度了，然后就是速度话题的封装和发布，海龟2也就可以动起来了。

所以这个例程的核心就是通过坐标系实现向量的计算，两只海龟还会不断运动，这个向量也得按照某一个周期计算，这就得用上TF的动态广播与监听了。

Launch文件解析
launch文件，里边启动了四个节点，分别是：

• 小海龟仿真器
• 海龟1的坐标系广播
• 海龟2的坐标系广播
• 海龟跟随控制
  两个坐标系的广播复用了turtle_tf_broadcaster节点，通过传入的参数名修改维护的坐标系名称

learning_tf/launch/turtle_following_demo.launch.py

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
 
 
def generate_launch_description():
    return LaunchDescription([
        Node(     #小海龟仿真器
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(   #海龟1的坐标系广播
            package='learning_tf',
            executable='turtle_tf_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
        DeclareLaunchArgument(  
            'target_frame', default_value='turtle1',
            description='Target frame name.'
        ),
        Node(    #海龟2的坐标系广播
            package='learning_tf',
            executable='turtle_tf_broadcaster',
            name='broadcaster2',
            parameters=[
                {'turtlename': 'turtle2'}
            ]
        ),
        Node(    #海龟跟随控制 监听海龟1与海龟2之间的位置关系并转换成速度指令通过话题发布出去，让海龟2跟随海龟1运动
            package='learning_tf',
            executable='turtle_following',
            name='listener',
            parameters=[
                {'target_frame': LaunchConfiguration('target_frame')}
            ]
        ), 
    ])

坐标系动态广播
海龟1和海龟2在world坐标系下的坐标变换，在turtle_tf_broadcaster节点中实现，除了海龟坐标系的名字不同之外，针对两个海龟的功能是一样的。

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')        # 创建一个海龟名称的参数 通过上面节点的参数不同会更换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.turtlename会被替换成turtle1或者turtle2
            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                    # 设置一个坐标变换的目标坐标系 self.turtlename会被替换成turtle1或者turtle2
        transform.transform.translation.x = msg.x                     # 设置坐标变换中的X、Y、Z向的平移
        transform.transform.translation.y = msg.y
        transform.transform.translation.z = 0.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

import math
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  # 监听坐标变换的监听器类
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_broadcaster = learning_tf.static_tf_broadcaster:main',
            'turtle_tf_broadcaster = learning_tf.turtle_tf_broadcaster:main',
            'tf_listener = learning_tf.tf_listener:main',
            'turtle_following = learning_tf.turtle_following:main',
        ],
    },