机器人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键，可以节省大量时间。