【ROS入门】TF与URDF

一、什么是TF

TF全程就是transform，就是一个坐标系的转换。在ROS中坐标的转换是一个很重要的内容，主要还是因为机器的不灵活性，如果是人，完全可以灵活地控制手臂去抓取一个物体，但是换作机器就没那么简单了，机器由很多零部件组成，每个零部件有一个自己的坐标系，就像下面这张图一样，每个零件根据自己的位姿，都可以建立一个三维坐标系，但是这个三维坐标系是根据自身的情况建立的，可以说是各自为政，但是零部件之间是间接或者直接连接的，也就是说可以根据连接关系，找出各个坐标系之间的转换关系，这个转换就是TF。

ROS中机器人模型包含大量的部件，这些部件统称之为link，每一个link上面对应着一个frame，即一个坐标系．link和frame概念是绑定在一起的。维护各个坐标系之间的关系，就要靠着tf tree来处理，维护着各个坐标系之间的联通。如下图：

TF树就是一个维护各个部件坐标关系的数据结构，每个坐标系都有一个父坐标系，利用这个父子关系来找出转换的方式，举个例子一辆车在路上行驶，根据自己的行驶方向可以以自身为原点建立一个坐标系，但是这个坐标系是以自己为核心的，也就是说从别人看是无法理解的，此时整个地球的坐标就可以看做父坐标系，让车的位置和自己的GPS信息之间建立联系，这样就可以将车坐标系中的位置信息转换为世界坐标系下的位置信息。上图中每一个圆圈代表一个frame，对应着机器人上的一个link，任意的两个frame之间都必须是联通的，如果出现某一环节的断裂，就会引发error系统报错。所以完整的tf tree不能有任何断层的地方，这样我们才能查清楚任意两个frame之间的关系。这一点从TF树的含义就可以看出，因为每个坐标系都需要和自己的父坐标系建立联系才可以转换，而兄弟坐标系间想要转换就只能经由父坐标系，推广到更大范围，想要整个TF树上的节点都可以进行坐标转换，就只能让整个树结构联通，只有这样才能进行转换。换一种方式去思考，将这个TF树看做一个无向图，两个节点之间联通，意味着存在一个转换路径让彼此坐标系建立联系。

二、TF工具

TF提供了很多的工具来帮助开发者调试和创建TF变换，主要有下面几种：
①tf_monitor
打印TF树中所有的坐标系的发布状态，也可以通过输入参数来查看指定坐标系之间的发布状态。

rosrun tf tf_monitor----查看所有坐标系的状态
rosrun tf tf_monitor 源坐标系名称 目标坐标系名称---- 查看指定两个坐标系之间的状态

②tf_echo
查看指定坐标系之间的变换关系

rosrun tf tf_echo 源坐标系名称 目标坐标系名称---- 查看指定两个坐标系之间的转换关系

这张图是两个小乌龟追逐的例子中使用tf_echo得到的消息，可以看出坐标的转换包括两部分，Translation表示的是三维坐标之间的转换关系，当两个小乌龟贴在一起的时候就全是0了。Rotation则表示角度的对应关系，包括四元数、旋转矩阵、欧拉角的转换关系。
③ static_transform_publisher
发布两个坐标系之间的静态坐标转换，这两个坐标系之间不发生相对位置的变化。
④view_frames
可视化调试工具，可以打印整个TF树结构的PDF文件。

rosrun tf view_frames----打印TF树的结构
evince frames.pdf----查看打印的PDF

这张图就是打印的结果，可以直接地看出TF树的结构。

三、TF转换代码

完整代码在此https://blog.csdn.net/weixin_43849505/article/details/120357533

下面拆开对每个部分单独记录一下，首先明确整个的过程，先是定义两个turtle节点，之后发布tf消息，利用父坐标系也就是世界坐标系来进行转换，从而实现彼此的坐标的转换。

对于TF广播器，在主函数中初始化节点和句柄，之后订阅乌龟的pose信息，这样每次收到pose信息之后就可以利用回调函数进行处理。程序的关键在回调函数，这个函数中需要初始化TF信息并且发送出去：

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    static tf::TransformBroadcaster br;
    // 根据乌龟当前的位姿 设置当前小乌龟相对于世界坐标系的坐标变换
    tf::StampedTransform transform;
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
    // 设置子坐标系在父坐标系下的位置信息 建立子坐标系的原点和父坐标系的对应关系
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);
    // 发布坐标变换 四个参数分别为 tf转换 时间戳 目标坐标系 待转换坐标系
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

首先初始化了一个TF广播器叫做br，之后准备了一个tf消息叫做transform，现在对这个消息进行初始化，这个消息的标准格式如下：

std_mags/Header header
        uint32 seq
        time stamp
        string frame_id
string child_frame_id
geometry_msgs/Transform transform
        geometry_msgs/Vector3 translation
                float64 x
                float64 y
                float64 z
        geometry_msgs/Quaternion rotation
                float64 x
                float64 y
                flaot64 z
                float64 w

可以看出主要的部分就是下面的三维坐标信息向量和表示旋转关系的四元数，所以代码里面用了c++中的TF数据类型进行了初始化。
看了四元数的知识之后回来补充一下，这里坐标的转换无非旋转加平移，旋转使用四元数表示，平移用平移向量来表示，其实也可以直接用一个变换矩阵来表示，但是出于节省空间的目的，这里用了冗余更少的坐标加四元数

之后就可以发布消息，发布的消息代表的是两个节点或者两个坐标系之间的转换关系，所以要指明发布的是哪两个坐标系之间的关系，除此之外还要加上时间戳和写好的tf消息。

对于TF监听器，主要的任务就是根据收到的TF消息，找出要转换的两个坐标系的关系。看代码，首先还是初始化节点，调用服务产生第二只乌龟，之后就是最关键的TF监听器部分。

 tf::TransformListener listener;
 
    ros::Rate rate(10.0);
    while (node.ok())
    {
        // 监听器的状态没有问题的情况下
        tf::StampedTransform transform;
        try
        {
            listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
            // 等待两个frame之间的联通 就是等待tf树上这两个点直接找到了转换的关系
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
            // 查找turtle2与turtle1的坐标变换 四个参数分别为目标坐标系 源坐标系 查询的时刻 转换关系的存放位置
        }
        catch (tf::TransformException &ex) 
        {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }
 
        // 根据turtle1和turtle2之间的坐标变换 计算turtle2需要运动的线速度和角速度
        // 并发布速度控制指令 使turtle2向turtle1移动
        // transform代表两个坐标系之间的转换关系
        geometry_msgs::Twist vel_msg;

        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),transform.getOrigin().x());
        // 计算角速度 使用getOrigin获得的是主控乌龟的原点在跟随乌龟坐标系下的位置 因为对这个程序而言乌龟就是原点 所以不需要额外的坐标变换
        // atan2函数返回以弧度表示的y/x的反正切
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
        // 计算线速度
        turtle_vel.publish(vel_msg);
 
        rate.sleep();
    }

首先初始化一个TF监听器和一个TF消息，这里的TF消息和TF广播器中的TF消息其实是一个东西，都是负责装载两个坐标系的对应关系，只不过在广播器中是先初始化消息在发布，在监听器中是接收消息后应用。之后调用监听器的waitForTransform函数，等待两个节点连通，这个函数有四个参数，分别为目标坐标系、源坐标系、时间戳和时间间隔，等待两个节点连通之后，就可以利用lookupTransform函数，去查询这两个坐标系的转换关系，这个函数和waitForTransform前三个参数都是一样的，只不过第四个函数换成了TF消息，也就是让一个还没有内容的TF消息去接收查询到的转换关系。这里要注意，两个函数使用的时间戳都是Time(0)而不是now，因为使用now还会有一个时间上的误差，而Time(0)则直接使用最新的一个转换关系，就不存在误差了。

也就是说，每个TF消息，就可以查询到两个节点之间的转换关系，那么一组TF消息，就组成了一棵树的转换关系，这种树的关系tf/tfMessage.msg或者tf2_msgs/TFMessage.msg来表示，其格式如下：

geometry_msgs/TransformStamped[] transforms
        std_msgs/Header header
                uint32 seq
                time stamp
                string frame_id
        string child_frame_id
        geometry_msgs/Transform transform
                geometry_msgs/Vector3 translation
                        float64 x
                        float64 y
                        float64 z
                geometry_msgs/Quaternion rotation
                        float64 x
                        float64 y
                        flaot64 z
                        float64 w

可以看出里面有一个TransformStamped组成的数组，用来存储多对转换关系，以此组成一棵TF树。

现在知道了两个坐标系的转换关系，就可以利用这个关系去转换坐标，直接使用TF消息的getOrigin函数，获得turtle1坐标系的原点，在turtle2坐标系下的位置，这个位置就是我们要跟随的位置，剩下的就是计算速度了。

现在完整总结一下这个过程，TF广播器不断向TF话题中发送TF消息，告知每两个节点之间的坐标转换关系，TF消息足够建立TF树之后，就可以根据TF树随意查询两个节点之间的转换关系，此时就利用TF监听器，等到自己要找的两个节点已经连通之后，得到转换的TF消息，确定移动方向然后移动即可。

四、统一机器人描述格式URDF

URDF中文名称统一机器人描述格式，使用XML格式描述机器人文件。主要是用来描述一个机器人的结构组成，利用xml文件的结构关系，去表示机器人的结构关系。
link和joint就是URDF中最重要的两个元素，link表示部件，而joint表示连接用到的关节。