ROS入门-15.ROS中的坐标管理系统

介绍ROS中的常用组件，本将介绍ROS中的坐标系管理系统TF

机器人中的坐标变换

坐标系变换是机器人基本理论当中非常重要的一块，比如说有A,B两个坐标系，这两个坐标系之间的关系可以通过向量去做描述，这里会包括平移和旋转的变换，所以任意两个坐标系之间的坐标变换可以通过一个4×4的矩阵去描述他的平移和旋转

在ROS中针对坐标变换的一个很重要的工具的使用方法，比如说是像这样的机器人，里面可能会有非常多的坐标系，呢么我们应该怎样去描述任意两个坐标系之间的关系是比较麻烦的，比如机器人上面的摄像头的坐标系跟 机器人底盘坐标系之间的位置关系是什么样的，如果用纯理论算的话可能中间有几十个坐标系之间的变换关系要做几十次的运算，但是在ROS当中就可以用TF这个功能包，它给我们提供了很重要的功能就是来管理所有坐标系，我们用这个功能包就可以直接通过查询的方式告诉我们这两个坐标系之间的关系什么样的，所有的底层的数学计算都封装到里面帮我们完成了。
TF是在ROS里面用的非常多的一个功能包，它主要能帮我们做这样几件事

它有一个时间属性，默认会记录十秒钟之内机器人所有坐标系之间的位置关系，比如
1.可以查询五秒钟之前，机器人头部坐标系相对于全局坐标系的关系是什么样的？（只要在10秒范围内都是可以查询到的）
2.另外就是，机器臂要去抓取一个物体，这个物体相对于机械臂终端的坐标系，相对机器人底盘的坐标系的位置关系是什么样的都可以通过TF功能包直接获取到
这就是TF功能包，它能够帮助我们去实现机器人里面比较繁杂的坐标系之间的管理和查询，非常重要
呢么TF坐标系的变换怎么样去实现，再从实现的底层机制来讲，它是一个广播和监听的机制，并不是前面讲到的topic和service，在所有的ROS系统里只要启动ROS Master启动TF之后，就会在后台去维护一个叫做TF树，我们所有坐标系都是通过树形结构保存在TF树，想去查询任意两个坐标系之间的变换关系的话直接可以通过这个树去查询得到。这就是TF在整个ROS当中实现的基本原理
下面来举个例子说明TF的简单的应用案例，

这是一个移动机器人有一个底盘，一般我们建模的时候会给底盘的中心点做一个坐标系，比如说这个坐标系以中心为零点，横向为X轴，纵向Y轴，称这个坐标系为base_link，在地盘上装了一个激光雷达，激光雷达因为要往外去发射激光点来获取它的一个深度信息，所以这个激光点，比如说我们是从激光雷达中心点发出去的，前面有一堵墙，呢么从发射点到墙之间的距离就会认为是我们雷达相对于障碍物之间的距离，呢这个点我们会认为它是一个base_laser，即激光雷达它所在的坐标系，呢所有检测到的激光雷达的数据其实是在激光雷达坐标系下的一个位置关系。这样就产生了两个坐标系，这两个坐标系是有一个位置关系的，我们可以通过机器人的安装位置来确定，比如说这个位置上下是Y方向平移有20cm，X方向有10cm平移，这样基本就可以两个坐标系之间有这样的平移关系，然后旋转关系是没有的
所以当我的雷达检测到障碍物之间有这样的距离0.3米，呢么我们怎样去把他变换得到在base_link坐标系下的数据，这是我们通过TF可以得到的

因为很多情况下，我们在做机器人算法的时候，我们关注的更多是在base_link下的距离而不是在base_laser下的距离
这是一个简单的只有两个节点组成的树，我们可以通过TF来管理这样两个坐标系，这是TF的一个基本实现的原理
接下来通过一个实际的海龟案例来进一步讲解TF在整个ROS当中起到作用，一起来运行这个历程，有两个海龟，运行之后会有这样的效果就是两个海龟出现之后，一只海龟在中心点，另外一个海龟实在下方，这只绿色海龟会一只向上走，会跟前面的海龟保持位置一致，我们可以通过键盘来控制中间这只海龟让他去运动，另外一只绿色海龟虽没有控制它但它会自动的去跟随所控制的海龟同步去做运动，这是通过坐标变换来实现的例子

sudo apt-get install ros-noetic-turtle-tf

1. 在运行之前需要去安装这样一个turtle-tf的示例的功能包，打开终端输入命令；所有ROS里功能包的安装都是ros-ros版本

roslaunch turtle_tf turtle_tf_demo.launch

**2.**运行，需要启动该指令，新指令roslaunch，是用来启动一个在ROS中的launch文件，该文件可以理解为一个脚本，里面可以去把很多节点启动写进去可以一次性启动很多节点
【这里因为是noetic版本报错ImportError:No module named yaml,然后只出现了一只海龟，这是python版本导致的，查到并实践成功的方法是
（1）进入usr.bin目录

cd /usr/bin

(2)运行语句删除原默认编译器文件

sudo rm -r python

(3)复制python3编译器设置为原编译器文件

sudo cp python3 python

借鉴原博客地址
再次输入该指令即可成功出现两只海龟

rosrun turtlesim turtle_teleop_key

**3.**再打开一个终端输入指令来运行海龟的键盘控制节点，就可以通过键盘控制第一个海龟运动，第二个海龟会跟着运动；这里只是位置跟随，角度并没有跟随

rosrun tf view_frames

4. 看一下当前ROS环境当中TF是什么样的，介绍第一个小工具 view_frames；这是在TF功能包里给我们提供的一个小工具可以直接可视化看到整个系统当中所有TF之间的关系，也是后面去调试机器人里面会经常用到的一个工具
输入指令后，他会监听5秒钟时间，会把5秒钟之内所有坐标系之间的关系都给保存下来
【再次出现错误

原因是python2和python3的语法不兼容，修改方法
（1）找到view_frames文件的目录
主目录/其他位置/计算机/opt/ros/noetic/lib/tf/view_frames

(2)在该目录下右键终端打开，输入以下命令打开修改文件权限，然后打开view_frames文件，找到报错第89行

sudo chmod 666 view_frames

（3)修改第89行为m = r.search(str(vstr))并保存

（4）关闭文件；输入指令关闭打开的权限

sudo chmod 777 view_frames

（5）再次输入rosrun tf view_frames，告诉输出pdf文件成功，默认放在主文件夹下
】
这就是当前系统当中的TF坐标间的位置关系，在当前的两只海龟的仿真器当中一共有三个坐标系，一个叫做world是全局坐标系，表示的是整个仿真器的坐标原点也就是左下角的原点，他是仿真器的零点是不会动的，另外还有两个坐标系叫做turtle1和turtle2分别是位于两只海龟上面的，所以大家想象海龟抽象一下，每只海龟其实是一个坐标系，所以现在抽象出来之后它的底层原理就是一共有三个坐标系，三个坐标系之间的位置关系就是刚才所展现的历程的底层实现的原理，两个坐标系turtle1和turtle2会不断地去重合到一起，这两个坐标系相对于world来讲是不断在变化的因为我们通过键盘在不断的控制turtle1，而turtle2会自动跟随turtle1，所以他俩相对world来讲是变化的，当然turtle1和turtle2的相对位置也是变化的，最终的目的是希望turtle2相对于turtle1坐标系之间的关系是重叠的，至少是在平移方向是重叠的，这是我们要通过坐标系来实现这个历程的一个目标
这就是介绍的一种工具，通过这种工具可以分析出来，其实这个问题可以抽象为turtle2坐标系向turtle1坐标系变换的底层原理，在后面做移动机器人，导航，建图等都会经常用到类似这样工具来看一下TF是否连通，比如如果turtle2和world箭头没有，呢说明这两个坐标系之间的位置没有建立成功。整个就是TF树，它是在ROS后台一直维护的，任意节点都可以通过查询这个树来获取任意两个坐标系之间的位置关系，该树并不是二叉树，他的节点可以有很多个。
5. 接下来介绍TF的第二个工具tf_echo，这个工具会更直接一点他可以直接去帮助我们去查询在树当中任意两个坐标系之间的位置关系

rosrun tf tf_echo turtle1 turtle2
查询turtle1和turtle2之间的关系，前者是根坐标系，后者是目标坐标系

通过控制键盘让海龟运动可以看到里面的数据是会发生变化的，这里是描述turtle2坐标系怎样通过变换变成到turtle1坐标系，里面数据会包含两部分一个是Translation，平移，就是坐标系在X,Y,Z三个方向上的平移，另一个是Rotation，旋转，就是坐标系怎样旋转变成一样的姿态，分为三行，三行的数据内容其实是等价的，只不过通过三种方式来做描述，第一种是通过Quaternion，就是四元数，通过x,y,z,w四元数方式来描述姿态；第二行是 RPY ，就是分别围绕X轴,Y轴，Z轴三轴旋转通过弧度单位来描述；第三行是RPY ，它通过三个坐标系的旋转的角度单位来描述

可以看到由于两个坐标系都是平面的所以他只有围绕Z轴有旋转关系，所以我们可以通过tf_echo这个工具可以直接获取到任意两个坐标系之间的位置关系的
6.第三个工具rviz更可视化一些，他是一个三维可视化的显示平台在ROS中用的非常多

rosrun rviz rviz -d ` rospack find turtle_tf` /rviz/ turtle_rviz.rviz

指令启动rviz可以看到两个坐标系之间的可视化关系


修改Fixed Frame为world；点击左下角add添加TF；黄色线根源是world坐标系，连接的是两个海龟坐标系，turtle1移动， turtle2不断追赶turtle1；现在要做的就是让 turtle2的坐标系不断向 turtle1移动，可以想象一下就是两个坐标系之间连接一根向量，这个向量表示的就是这两个坐标系之间的位置关系，只要让 turtle2坐标系沿着这根向量向 turtle1走就可以到 turtle1位置；其实这个向量就是刚通过tf_echo打印出来的数据，这个数据包括x,y,z以及它的姿态，这就是该向量的描述数据，有这样的数据后通过简单的计算，比如说通过这个向量的长度得到两个坐标系之间的距离，距离再除以设置的时间就是速度，然后就可以把速度直接发给海龟去做速度指令的运动了，这就是底层实现的基本的原理


下面公式是在从底层原理上来讲，turtle2和turtle1之间的位置关系可以通过两者相对于world关系来得到，是两个4×4矩阵的乘法可以得到