机器人TF坐标系变换与一些可视化工具的应用

TF坐标在ROS中是一个非常重要的概念，因为机器人在做日常操作任务的时候，对于其所在位置和朝向是需要时刻知道的，而机器人是由很多节点组成的协同任务，对于每个部件，我们需要知道它的位姿(位置和朝向)，这使得坐标系就成为了一个很重要的问题。
TF的功能就是能够换算出该点在其他坐标系下的坐标。比如，以机器人为例，定义两个坐标系，一个坐标系以机器人移动平台的中心为原点，称为base_link参考系，另一个坐标系以激光雷达的中心为原点，称为base_laser参考系，当然这个命名是需要唯一。利用TF坐标转换，将base_laser参考系与base_link参考系重合，这样就可以准确得知机器人自身位置，便于继续进行下一步的操作。

1、术语概述

那么在介绍坐标系的时候，会经常看到位置、朝向，位姿等术语，先介绍下在三维世界中的几个术语：
位置：是一个三维的向量(x,y,z)，用于表示相对于原点沿着各个轴的方向分别移动了多远。
朝向：也是一个三维向量(roll,pitch,yaw)，用于表示分别绕各个轴转动了多少。
位姿：将位置和朝向放在一起，一对(位置,朝向)表示一个位姿。这种具有六维(三维位移,三维旋转)的位姿成为6D位姿。
给定两者之间的位姿，我们可以变换两个坐标系下的数据，这个变换通常是一些矩阵乘法。有的位姿是动态的，有的位置是固定的，比如说，固定在机器人身上的激光雷达，就是相对固定的；而机械手的位姿就是不断变化的，因为需要移动，旋转等和做一些抓取动作。
为了管理这些坐标系和变换，就设计了这个tf(Transform的缩写)包来实现，使用的是分布式的方式，用ROS话题来共享变换的数据。任何节点可以发布某些变换的当前信息，并且任何节点都可以订阅变换的数据，从不同的发布源得到机器人各部件之间的所有变换关系。

2、tf坐标系

对于一个强大系统来说，tf会比较的复杂，出错的可能性较大，所以就出现很多关于tf相关的检查和调试工具，帮助你理解正在发生的实情，我们接下来看下有哪些可视化的工具可供使用。

2.1、view_frames

我们先启动海龟示例，对于前面安装过的可以忽略，也可以使用 sudo apt install ros-melodic-turtle-tf 进行安装，对于ros的安装，推荐大家可以看下前面介绍的一篇文章：Ubuntu18.04版本安装ROS及出现错误的处理方法

启动海龟

roslaunch turtle_tf turtle_tf_demo.launch

启动键盘操作

rosrun turtlesim turtle_teleop_key

然后再开一个终端运行命令：rosrun tf view_frames

the rosdep view is empty: call 'sudo rosdep init' and 'rosdep update'
Listening to /tf for 5.0 seconds
Done Listening
dot - graphviz version 2.40.1 (20161225.0304)

Detected dot version 2.40
frames.pdf generated

这里生成了一个坐标系的pdf文件frames.pdf，在后面看到的frame也都表示为坐标系的意思。

2.2、rqt_tf_tree

对于上面这个view_frames命令，运行后保存的是当前坐标系，不过这样的保存是离线的，不能实时反映变化的坐标关系。
我们换一个命令：rqt_tf_tree，可以显示tf坐标系的树结构

rosrun rqt_tf_tree rqt_tf_tree

运行上面命令将会弹出可视化界面，如下图：

可以看到，显示了录制时间，两个海龟对应的广播(发布)节点，平均速率、缓冲大小，最近和最旧的转换时间，还可以高亮显示，鼠标放在节点上面，相关节点将显示不同颜色。
我们可以操作海龟，然后点击左上角的刷新，就会看到速率等都发生了变化，这样看起来就比较直观。如下图：

TF建立的树状结构的坐标系，在整个坐标系是没有闭环的，也就是说每个坐标系，只能有一个父坐标系，但可以有多个子坐标系。

2.3、tf_echo

命令格式：rosrun tf tf_echo <source_frame> <target_frame> 
显示的信息是从source_frame到target_frame的旋转平移变换。

查看两只海龟参考系之间的关系

rosrun tf tf_echo turtle1 turtle2

At time 1652155224.053
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.893, 0.450]
            in RPY (radian) [0.000, -0.000, 2.208]
            in RPY (degree) [0.000, -0.000, 126.509]
At time 1652155225.061
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.893, 0.450]
            in RPY (radian) [0.000, -0.000, 2.208]
            in RPY (degree) [0.000, -0.000, 126.509]
At time 1652155226.053
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.893, 0.450]
            in RPY (radian) [0.000, -0.000, 2.208]
            in RPY (degree) [0.000, -0.000, 126.509]

2.4、static_transform_publisher

两个坐标系之间的静态坐标变换，这两个坐标系不发生相对位置变化。比如说，无人车上的激光雷达与IMU惯性测量模块，这两者基本是固定的，所以可以使用static_transform_publisher来发布这两者的坐标变换。
命令格式如下：
第一种命令格式：

static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms

第二种命令格式：

static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms

上面的两种格式，需要设置坐标的偏移和旋转参数
偏移参数：都使用相对于x y z三轴的坐标位移。
旋转参数：其中第一种命令格式使用以弧度为单位的 yaw pitch roll 三个角度(yaw是围绕x轴旋转的偏航角，pitch是围绕y轴旋转的俯仰角，roll是围绕z轴旋转的翻滚角）
第二种命令格式使用四元数表达旋转角度。
period_in_ms发布频率以毫秒为单位，一般100ms也就是0.1秒比较合适。

在launch中使用方法示例如下：

<launch>
    <node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 link1_parent link1 100" />
</launch>

2.5、roswtf

roswtf是一个插件，分析你当前的tf配置并试图找出常见问题。
比如输入命令，本人当前状态显示如下信息，有错误等情况都会高亮显示：

the rosdep view is empty: call 'sudo rosdep init' and 'rosdep update'
No package or stack in the current directory
================================================================================
Static checks summary:

Found 1 error(s).

ERROR ROS Dep database not updated: Please update rosdep database with 'rosdep update'.
================================================================================
Beginning tests of your ROS graph. These may take a while...
analyzing graph...
... done analyzing graph
running graph rules...
connection to [/tf_echo_1652154589994246734] timed out
... done running graph rules

Online checks summary:

Found 1 warning(s).
Warnings are things that may be just fine, but are sometimes at fault

WARNING The following node subscriptions are unconnected:
 * /tf_echo_1652154589994246734:
   * /tf_static
 * /turtle_pointer:
   * /tf_static

Found 2 error(s).

ERROR Could not contact the following nodes:
 * /tf_echo_1652154589994246734

ERROR Errors connecting to the following services:
 * service [/tf_echo_1652154589994246734/set_logger_level] appears to be malfunctioning: Unable to communicate with service [/tf_echo_1652154589994246734/set_logger_level], address [rosrpc://jetson-desktop:54551]
 * service [/tf_echo_1652154589994246734/get_loggers] appears to be malfunctioning: Unable to communicate with service [/tf_echo_1652154589994246734/get_loggers], address [rosrpc://jetson-desktop:54551]

3、tab键

tab键，能够将代码补全或显示有关输入命令的未完成命令，双击tab键可以提高效率和确保输入的命令是无误的。
比如输入：rosrun rqt_   然后双击tab键，将自动出现关于rqt开头的各种节点：

rqt_action           rqt_logger_level     rqt_robot_monitor
rqt_bag              rqt_moveit           rqt_robot_steering
rqt_bag_plugins      rqt_msg              rqt_runtime_monitor
rqt_console          rqt_nav_view         rqt_rviz
rqt_dep              rqt_plot             rqt_service_caller
rqt_graph            rqt_pose_view        rqt_shell
rqt_gui              rqt_publisher        rqt_srv
rqt_gui_cpp          rqt_py_common        rqt_tf_tree
rqt_gui_py           rqt_py_console       rqt_top
rqt_image_view       rqt_reconfigure      rqt_topic
rqt_launch           rqt_robot_dashboard  rqt_web

如果内容显示特别多，还会提示，比如我只输入：rosrun然后双击tab键
Display all 229 possibilities? (y or n)
可以选择是否继续显示。

如果输入的命令接下来是确定性匹配，这个时候双击tab键将自动补全剩余的。
比如输入：rosrun rqt_gu
双击tab键将会自动补全成rosrun rqt_gui，我们再次双击tab键，将列出rqt_gui开头的所有匹配节点：

rqt_gui      rqt_gui_cpp  rqt_gui_py

4、可视化工具

4.1、rqt_graph

rqt_graph可以将计算图可视化，输入命令：rosrun rqt_graph rqt_graph
如果没有启动节点管理器而直接运行这条命令则会报错：
Could not find ROS master,Either start a 'roscore' or abort loading the plugin.

所以我们在此之前，先启动节点管理器，也是以海龟为示例，然后运行上面那条命令，如下图： 

可以直观的看到两个海龟节点，里面有pose位姿，键盘操作等话题，然后位姿通过各自的广播来发布信息，中间有一个就是我们这节重点讲解的tf坐标系的话题topic:/tf（具备订阅和发布功能）来变换坐标，最后通过节点/turtle_pointer到/rosout输出。
对于图中有哪些节点，我们也可以通过命令来获取：rosnode list

/rosout
/sim
/teleop
/teleop_turtle
/turtle1_tf_broadcaster
/turtle2_tf_broadcaster
/turtle_pointer

对于节点可以看做是网站中的各自端口对应的应用，所以我们也可以像ping网络一样的来查看：
rosnode ping /rosout

rosnode: node is [/rosout]
pinging /rosout with a timeout of 3.0s
xmlrpc reply from http://jetson-desktop:46881/    time=1.574039ms
xmlrpc reply from http://jetson-desktop:46881/    time=1.164913ms
...

rosnode ping /sim

rosnode: node is [/sim]
pinging /sim with a timeout of 3.0s
xmlrpc reply from http://jetson-desktop:36081/    time=1.548052ms
xmlrpc reply from http://jetson-desktop:36081/    time=1.078129ms
...

4.2、rqt_topic 

rqt_topic查看话题的一个插件，能够知道各节点话题的具体实时变化情况，输入命令：

rosrun rqt_topic rqt_topic

如下图：

可以看到，每个话题的详细信息，有Type消息类别、Bandwidth带宽、Hz频率、Value值。
我们点开/tf坐标系查看下里面有哪些属性，如下图：

可以看到，指明了父坐标和子坐标，当然主要还是坐标的转换，其中时间戳有两个，一个纳秒和一个秒为单位的时间。 

 4.3、rqt_publisher

rqt_publisher用来发布具有固定或计算字段值的任意消息。输入命令：
rosrun rqt_publisher rqt_publisher
将会弹出如下图的界面：

topic话题和type消息数据类型，都可以点击下拉框进行选择，频率的输入，然后点击+号添加进来。如下图： 

然后就可以修改里面的expression字段对应的值，手动进行输入调试。

4.4、rviz

rviz是ROS自带的一个图形化工具，可以方便的对ros的程序进行图形化操作。输入命令：rviz
如下图：

主要分5个功能区，如图中所标识的。其中工具栏中的Set initial pose、Set target pose、Publish location point：一般在建图导航时使用。

添加显示操作如图：

点击左下角的Add按钮，将会弹出一个界面，然后可以选择“By display type”通过显示类型来添加，不过需要自己修改对应话题，坐标系才可以显示出来；或者就是选择“By topic”通过话题的方式，直接添加就可以正常显示。最后点击右下角的OK按钮即可

最后温馨提示，在使用ROS核心工具和其他Linux命令的时候，常记得敲tab键，可以节省大量时间。