ROS(九)：坐标系统tf

1.安装ｔｕｒｔｌｅ包

rosdep install turtle_tf rviz

2.运行demo

roslaunch turtle_tf turtle_tf_demo.launch

可以看到画面上有两个小乌龟，可以在终端激活的情况下用键盘控制小乌龟一号，黄色的那只，绿色的小乌龟就会紧紧的跟在黄色小乌龟后面．

这里写图片描述

３．ｄｅｍｏ的分析

这个例程使用tf建立了三个参考系：a world frame, a turtle1 frame, and a turtle2 frame。然后使用tf broadcaster发布乌龟的参考系，并且使用tf listener计算乌龟参考系之间的差异，使得第二只乌龟跟随第一只乌龟。

我们可以使用tf工具来具体研究。

rosrun tf view_frames

这里写图片描述

该文件描述了参考系之间的联系。三个节点分别是三个参考系，而/world是其他两个乌龟参考系的父参考系。还包含一些调试需要的发送频率、最近时间等信息。

tf还提供了一个tf_echo工具来查看两个广播参考系之间的关系。我们可以看一下第二只得乌龟坐标是怎么根据第一只乌龟得出来的。

rosrun tf tf_echo turtle1 turtle2

这里写图片描述

$T_{turtle1\_turtle2} = T_{turtle1\_world}*T_{world\_turtle2}$

这个公式的解释是２号小乌龟到１号小乌龟的坐标转换等于世界坐标系到１号小乌龟的Ｔ乘以２号小乌龟到世界坐标系转换的Ｔ．

４．写一个ｔｆ的广播

创建一个功能包,并添加对应的以来

 cd %YOUR_CATKIN_WORKSPACE_HOME%/src
 catkin_create_pkg learning_tf tf roscpp rospy turtlesim
 cd %YOUR_CATKIN_WORKSPACE_HOME%/
 catkin_make
 source ./devel/setup.bash

编写对应的源代码

cd /home/felaim/catkin_ws/src/learning_tf/src

添加对应的源码turtle_tf_broadcaster.cpp

//
// Created by felaim on 4/11/18.
//

#include <ros/ros.h>
//tf软件包提供了TransformBroadcaster的实现，以帮助简化发布转换的任务。
// 要使用TransformBroadcaster，我们需要包含tf/transform_broadcaster.h头文件
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr &msg) {
    //创建一个TransformBroadcaster对象,用来发布变换矩阵
    static tf::TransformBroadcaster br;
    tf::Transform transform;
    //设置２D的小乌龟的位姿，所以Z设置为零
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);
    //这是真正的工作完成的地方,使用TransformBroadcaster发送变换需要四个参数
    //第一个参数是变换矩阵
    //给发布的变换一个时间戳,地拥护发就是当前时间ros::Time::now()
    //设定父亲框架链接的名称为"world"
    //设定子框架链接的名称为,就是小乌龟的名字
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "my_tf_broadcaster");
    if (argc != 2) {
        ROS_ERROR("need turtle name as argument");
        return -1;
    };
    turtle_name = argv[1];

    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name + "/pose", 10, &poseCallback);

    ros::spin();
    return 0;

}

在CMakeLists.txt文件中添加下面的语句:

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

重新catkin_make一下.

建立一个launch文件夹,添加start_demo.launch

<launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    <!-- Axes -->
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>

    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle2" name="turtle2_tf_broadcaster" />

  </launch>

运行launch文件

roslaunch learning_tf start_demo.launch

检查结果

rosrun tf tf_echo /world /turtle1

输出的结果是1号小乌龟到世界坐标系的tf.

这里写图片描述

在tf中还有几个问题注意的:

最常用的就是map，odom，base_link，base_laser坐标系，这也是开始接触gmapping的一些坐标系。

map:地图坐标系，顾名思义，一般设该坐标系为固定坐标系（fixed frame），一般与机器人所在的世界坐标系一致。

base_link:机器人本体坐标系，与机器人中心重合，当然有些机器人(PR2)是base_footprint,其实是一个意思。

odom：里程计坐标系，这里要区分开odom topic，这是两个概念，一个是坐标系，一个是根据编码器（或者视觉等）计算的里程计。但是两者也有关系，odom topic 转化得位姿矩阵是odom–>base_link的tf关系。这时可有会有疑问，odom和map坐标系是不是重合的？（这也是我写这个博客解决的主要问题）可以很肯定的告诉你，机器人运动开始是重合的。但是，随着时间的推移是不重合的，而出现的偏差就是里程计的累积误差。那map–>odom的tf怎么得到?就是在一些校正传感器合作校正的package比如gmapping会给出一个位置估计（localization），这可以得到map–>base_link的tf，所以估计位置和里程计位置的偏差也就是odom与map的坐标系偏差。所以，如果你的odom计算没有错误，那么map–>odom的tf就是0.

base_laser:激光雷达的坐标系，与激光雷达的安装点有关，其与base_link的tf为固定的。

这个一定要注意,下标的区别.如果是视觉的话,相机坐标系和世界坐标系, $T_{cw}$ 和 $T_{wc}$ 的区别.

5.tf的listener

在learning_tf/src中添加源码:

//
// Created by felaim on 4/11/18.
//

#include <ros/ros.h>
//tf软件包提供了一个TransformListener的实现来帮助简化接收转换的任务。
// 要使用TransformListener，我们需要包含tf/transform_listener.h头文件
#include <tf/transform_listener.h>

#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char **argv) {
    ros::init(argc, argv, "my_tf_listener");

    ros::NodeHandle node;

    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");

    turtlesim::Spawn srv;
    add_turtle.call(srv);

    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    //在这里，我们创建一个TransformListener对象。 一旦创建了监听器，它就开始接收通过线路进行的tf转换，并缓冲它们长达10秒。
    // TransformListener对象的范围应该保持不变，否则它的缓存将无法填充，几乎每个查询都会失败。
    // 一种常见的方法是使TransformListener对象成为类的成员变量
    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while (node.ok()) {
        tf::StampedTransform transform;
        try {
            //在这里,真正的工作完成了,我们向侦听器查询特定的转换.
            //我们希望1号小乌龟到2号小乌龟的转换
            //我们想要改变的时间.提供ros::Time(0)只会为我们提供最新的可用转换
            //所有这些包装在try-catch块中以捕捉可能的异常
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) {
            ROS_ERROR("%s", ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        //在这里,变换用于计算turtle2的新的线性和角速度,基于它鱼turtle1的距离和角度.
        //新速度发布在主题"turtle2/cmd_vel"中,sim将使用它来更新turtle2的运动
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().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();
    }

    return 0;

};

在CMakeLists.txt中添加对应的可执行文件和链接库

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

重新catkin_make

在之前的start_demo.launch文件中添加

  <launch>
    ...
    <node pkg="learning_tf" type="turtle_tf_listener"
          name="listener" />
  </launch>

运行launch文件:

roslaunch learning_tf start_demo.launch

这里写图片描述

刚开始会报出上图的error
发生这种情况是因为listener在收到有关2号乌龟消息之前计算转换,在使用turtlesim产生第二个小乌龟并开始广播一个tf帧是需要时间,所以找不到对应帧.

6添加一个frame

在很多应用中,添加一个参考系是很有必要的,比如在一个world参考系下,有一个RGBD相机节点,tf可以帮助我们将rgbd扫描的信息坐标换成全局坐标,然后可以在全局中显示对应的位置.

a.tf消息结构
tf中的信息是一个树状的结构，world参考系是最顶端的父参考系，其他的参考系都需要向下延伸。如果我们在上文的基础上添加一个参考系，就需要让这个新的参考系成为已有三个参考系中的一个的子参考系。

这里写图片描述

按照之前的方法,在src/frame_tf_broadcaster.cpp

//
// Created by felaim on 4/11/18.
//

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char **argv) {
    ros::init(argc, argv, "my_tf_broadcaster");
    ros::NodeHandle node;

    tf::TransformBroadcaster br;
    tf::Transform transform;

    ros::Rate rate(10.0);
    while (node.ok()) {
        //在这里，我们创建一个新的转换，从父turtle1到新的小胡萝卜1
        // carrot1框架距乌龟1框左侧2米
        transform.setOrigin(tf::Vector3(0.0, 2.0, 0.0));
        transform.setRotation(tf::Quaternion(0, 0, 0, 1));
//        transform.setOrigin(tf::Vector3(2.0 * sin(ros::Time::now().toSec()), 2.0 * cos(ros::Time::now().toSec()), 0.0));
//        transform.setRotation(tf::Quaternion(0, 0, 0, 1));

        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1") );

        rate.sleep();
    }

    return 0;
}

在CMakeLists.txt中添加对应的代码

add_executable(frame_tf_broadcaster src/frame_tf_broadcaster.cpp)
target_link_libraries(frame_tf_broadcaster ${catkin_LIBRARIES})

重新carkin_make.

在launch文档中添加对应的节点

  <launch>
    ...
    <node pkg="learning_tf" type="frame_tf_broadcaster"
          name="broadcaster_frame" />
  </launch>

运行对应的代码:

roslaunch learning_tf start_demo.launch

这时运行了还没有变化,在src/turtle_tf_listener.cpp修改一部分代码:

 listener.lookupTransform("/turtle2", "/carrot1",
                           ros::Time(0), transform);

重新编译并运行

 catkin_make
 roslaunch learning_tf start_demo.launch

这里写图片描述

将frame_tf_broadcaster.cpp文件进行修改，可以使turtle2在turlte1旁旋转。

    transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );

重新编译并运行

 catkin_make
 roslaunch learning_tf start_demo.launch

这里写图片描述

7. tf and Time

如何获得特定时间的变换矩阵?
打开src/turtle_tf_listener.cpp.
可以看到:

   try{
      listener.lookupTransform("/turtle2", "/carrot1",  
                               ros::Time(0), transform);

我们可以让2号小乌龟跟着1号小乌龟,修改下列代码:

//在这里,我们指定一个时间为0,
//对于tf而言,0表示在缓冲区中最新可获得的变换矩阵
   try{
      listener.lookupTransform("/turtle2", "/turtle1",  
                               ros::Time(0), transform);

这里写图片描述

可以看到2号小乌龟始终跟随1号小乌龟

然后,我们改成下面的代码:

 try{
    listener.lookupTransform("/turtle2", "/turtle1",  
                             ros::Time::now(), transform);

结果编译完进行运行会报如下的错误:

[ERROR] [1523497623.747414128]: Lookup would require extrapolation into the future.  Requested time 1523497623.747204825 but the latest data is at time 1523497623.743468028, when looking up transform from frame [turtle1] to frame [turtle2]

这是为什么呢?我们可以看到报错是时间有问题.每个listener都有一个缓冲区,用于存储来自不同tf的broadcaster的坐标变换.当broadcaster发出tf时,通常需要几毫秒的时间.因此,当我们”现在”请求tf的时候,应该等几毫秒在获取信息.

我们可以修改代码如下:

  try{
    ros::Time now = ros::Time::now();
    listener.waitForTransform("/turtle2", "/turtle1",
                              now, ros::Duration(3.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             now, transform);

上述代码设置了等待变换矩阵.waitForTransform()需要四个参数变量.1.目标帧, 2.源帧, 3.现在的时间, 4.最长等待时间间隔

注意：使用ros :: Time :: now（）就是这个例子。通常这将是希望被转换的数据的时间戳.

所以waitForTransform（）实际上会阻止，直到两个海龟之间的转换变为可用（这通常需要几毫秒），或者,如果转换不可用,直到达到超时.

重新编译并运行代码,还是可以看到如下的错误:

[ERROR] [1523500937.935210218]: Lookup would require extrapolation into the past.  Requested time 1523500934.925630513 but the earliest data is at time 1523500935.359631462, when looking up transform from frame [turtle1] to frame [turtle2]

发生这种情况是因为turtle2需要非零时间来产生并开始发布tf帧。所以，第一次你问现在/turtle2帧可能不存在，当请求变换时，变换可能还不存在，并且第一次失败。第一次转换后，所有转换都存在，乌龟的行为如预期。

8.Time travel with tf
现在,我们可以不要让2号小乌龟直接去1号小乌龟的地方,让2号小乌龟去5秒钟前1号小乌龟的位置

  try{
    ros::Time past = ros::Time::now() - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", "/turtle1",
                              past, ros::Duration(1.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             past, transform);

编译运行后,所以现在，如果你想运行这个，你期望看到什么？肯定在第一个5秒内第二只乌龟不知道该去哪里，因为我们还没有第一只乌龟的5秒历史。但是这5秒之后呢？让我们试试看：

这里写图片描述

2号小乌龟怕是被tf这个教程弄疯了…根本无法控制…
我们问的问题是，“5秒前/turtle1 5秒前相对于/turtle2 5秒钟前的姿势是什么？”。这意味着我们正在控制第二只乌龟，基于它在5秒前的位置以及第一只乌龟在5秒前的位置。

我们真正想要问的是：“相对于/turtle2的当前位置，5秒前/ turtle1的姿势是什么？”。

那如果要正确地问这样的问题,获得对应的答案呢?

  try{
    ros::Time now = ros::Time::now();
    ros::Time past = now - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", now,
                              "/turtle1", past,
                              "/world", ros::Duration(1.0));
    listener.lookupTransform("/turtle2", now,
                             "/turtle1", past,
                             "/world", transform);

使用进阶版的lookupTransform(),需要6个参数,1.目标帧,2.时间,3.源帧,4.时间,5.指定一个不会随时间变化的帧作为参考帧,这里可以选择/world,好像也没其他的…6.选择存储的变量

这里写图片描述
该图显示了后台正在做什么, 过去它计算从第一只乌龟到世界的转变, 在时间从过去到现在的世界框架中, 现在，我们计算从世界到第二只乌龟的变化.

虽然参数变多了,好歹2号小乌龟能够跟着1号小乌龟♪(^∇^*)

这里写图片描述

参考地址:
http://wiki.ros.org/tf/Tutorials/Adding%20a%20frame%20%28C%2B%2B%29

https://blog.csdn.net/flyinsilence/article/details/51854123

https://blog.csdn.net/dxuehui/article/details/39672871

https://www.ncnynl.com/archives/201702/1313.html

ROS(九)：坐标系统tf

猜你喜欢