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 

Tturtle1_turtle2=Tturtle1_world∗Tworld_turtle2 T t u r t l e 1 _ t u r t l e 2 = T t u r t l e 1 _ w o r l d ∗ T w o r l d _ t u r t l e 2

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

４．写一个ｔｆ的广播

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

 cd %YOUR_CATKIN_WORKSPACE_HOME%/srccatkin_create_pkg learning_tf tf roscpp rospy turtlesimcd %YOUR_CATKIN_WORKSPACE_HOME%/catkin_makesource ./devel/setup.bash

编写对应的源代码

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

添加对应的源码turtle_tf_broadcaster.cpp

//
 // Created by felaim on 4/11/18.
 //#include 
 //tf软件包提供了TransformBroadcaster的实现，以帮助简化发布转换的任务。
 // 要使用TransformBroadcaster，我们需要包含tf/transform_broadcaster.h头文件
 #include 
 #include 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文件

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为固定的。

这个一定要注意,下标的区别.如果是视觉的话,相机坐标系和世界坐标系, Tcw T c w 和 Twc T w c 的区别.

5.tf的listener

在learning_tf/src中添加源码:

//
 // Created by felaim on 4/11/18.
 //#include 
 //tf软件包提供了一个TransformListener的实现来帮助简化接收转换的任务。
 // 要使用TransformListener，我们需要包含tf/transform_listener.h头文件
 #include #include 
 #include 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("spawn");turtlesim::Spawn srv;add_turtle.call(srv);ros::Publisher turtle_vel = node.advertise("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文件:

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 
 #include 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文档中添加对应的节点

  ...

运行对应的代码:

roslaunch learning_tf start_demo.launch

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

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

重新编译并运行

 catkin_makeroslaunch 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_makeroslaunch 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