素材巴巴 > 程序开发 >

ROS(九):坐标系统tf

程序开发 2023-09-20 18:13:59

1.安装turtle包

rosdep install turtle_tf rviz

2.运行demo

roslaunch turtle_tf turtle_tf_demo.launch

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

这里写图片描述

3.demo的分析

这个例程使用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

这个公式的解释是2号小乌龟到1号小乌龟的坐标转换等于世界坐标系到1号小乌龟的T乘以2号小乌龟到世界坐标系转换的T.

4.写一个tf的广播

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

 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;//设置2D的小乌龟的位姿,所以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


标签:

素材巴巴 Copyright © 2013-2021 http://www.sucaibaba.com/. Some Rights Reserved. 备案号:备案中。