ROS1重温：TF变换

---

现实应用

• 机器人自身会有许多部件，基本以底座为基础，然后在上层增加传感器、机器臂等。在未经处理时，所有部件运动参数均以自身为坐标系，为了统一运动，一般会转换到 “底座坐标系” 或者 “世界坐标系”
• 机器人在运动中会遇到两点之间位置关系的问题，在该过程中，至少有一方时处于动态的，为了方便速度计算，也需要进行坐标转换，最少要知道 点与点之间的相对位置关系

---

TF库

• ROS 集成了机器人位姿变换的基础需求，可以直接调用相关接口实现坐标变换

---

主要库以及函数

坐标系发布

• 用到的库

  // 静态坐标发布
  #include "tf2_ros/static_transform_broadcaster.h"
  // 动态坐标发布
  #include "tf2_ros/transform_broadcaster.h"	// 动态坐标
  

• 用到的函数

  // 静态坐标发布对象声明
  tf2_ros::StaticTransformBroadcaster static_broadcaster;
  // 动态坐标发布对象声明
  tf2_ros::TransformBroadcaster dynamic_broadcaster;		
  	
  // 静态坐标发布、动态坐标发布均会使用到该函数
  void sendTransform(const geometry_msgs::TransformStamped & transform)
  

欧拉角转四元数

• 在发布坐标系的同时，可能会涉及到欧拉角到四元数的转换

  • 欧拉角（r, p, y）： 通俗简单讲，就是物体绕着 x、y、z三个轴旋转的角度，一般以弧度为单位，三个角度学名分别叫 （以 x 轴的方向为正前方） ：滚转角（Roll）、俯仰角（Pitch）、偏航角（Yaw）（如下图所示），旋转范围是 [-3.14, 3.14]

  • 滚转角
    

  • 俯仰角
    

  • 偏航角
    

  • 四元数（x, y, z, w） ：可以理解是对三维坐标的更具体化表示，就像二维坐标(x, y)，用三维坐标表示就是(x, y, 0)。相应的三维坐标(x,y,z)，用四维坐标表示就是(x, y, z, 0)。

  • 欧拉角转四元数所用到的库：

    #include "tf2/LinearMath/Quaternion.h"
    

  • 欧拉角转四元数所用到的API：

    // 创建四元数对象
    tf2::Quaternion qtn;
    // 欧拉角转四元数
    qtn.setRPY(r, p, y);	// 填入欧拉角
    
    // 获得四元数
    x = qtn.getX();
    y = qtn.getY();
    z = qtn.getZ();
    w = qtn.getW();
    

坐标监听

• 为了转换坐标系，我们需要不断监听获取要转换的目标坐标系（即小乌龟位姿新的参考对象）
• 坐标系的监听涉及两个方面：

  • 监听缓冲队列： 用于存储监听到的坐标信息

    • 涉及的库

      #include "tf2_ros/buffer.h"
      

    • 队列声明方式（其实就这么简单，只需要声明一个变量）

      // 创建监听队列
      tf2_ros::Buffer buffer;
      

  • 监听者： 就是监听对象

    • 涉及的库

    #include "tf2_ros/transform_listener.h"
    

    • 监听者声明方式

    tf2_ros::TransformListener transform_listener(buffer);
    

坐标系转换

• 这算是 TF 库的核心函数了
• 涉及到的库

#include "geometry_msgs/TransformStamped.h"

• 涉及到的函数

  geometry_msgs::TransformStamped 
    lookupTransform(const std::string& target_frame, const std::string& source_frame,
		    const ros::Time& time) const;

• 官方函数说明

* \brief Get the transform between two frames by frame ID.
* \param target_frame The frame to which data should be transformed
* \param source_frame The frame where the data originated
* \param time The time at which the value of the transform is desired. (0 will get the latest)
* \return The transform between the frames
*
* Possible exceptions tf2::LookupException, tf2::ConnectivityException,
* tf2::ExtrapolationException, tf2::InvalidArgumentException

中文释义：
*\brief 按坐标系 ID 获取两帧数据之间的转换。

*\param target_frame 数据要转换到的坐标系

*\param source_frame 数据源所在的坐标系

*\param time 需要转换值的时间。（时间设置为 0 将获取最新情况）

*\return 坐标之间的相对位置变换关系

*

*可能的异常tf2:：LookupException，tf2:：ConnectivityException，

*tf2:：ExtrapolationException，tf2:：InvalidArgumentException

• 使用实例：
  (由于在变换之前需要接收到坐标信息，所以必须保证时间及频率上的稳定一致，所以需要加入抛异常，以及睡眠等待)

// 设置频率
    ros::Rate r(10);
    while (ros::ok())
    {
        try
        {
            // 参考系转变，获取量参考系之间的相对位置关系
            geometry_msgs::TransformStamped coordinate_relative_position 
                = buffer.lookupTransform("target_frame_ID", "source_frame_ID", ros::Time(0));
        }
        catch (const std::exception &e)
        {
            ROS_WARN("异常消息：%s", e.what());
        }
        r.sleep();	// 设置休眠，保证时间戳对齐
    }