[DAY 28] 回传 Gazebo 当中机器人之座标

前言

大家好，介绍完笔者团队的控制自走车研究後，今天我们则要来介绍如何在 Gazebo 世界中取得我们机器人的座标资讯。在实际找出座标资讯之前，我们也会先简单介绍一下 ROS 中座标系之间的关系，毕竟厘清这个概念对於 ROS 开发来说也是不可或缺的环节，那麽以下就直接进入今天的内容吧!

座标系

在介绍 .xacro 档时我们使用了上图来解释 <link> 与 <joint> 之间的关系，而今天我们再将这张图做进一步的延伸，用来解释定义座标系的重要性：

左图为静止状态中机器人各个部件的座标轴位置，假设今天机器人想要拿取一个放在桌上的物体，可以看到机器人的两节手臂在动作时，其座标轴 arm_1、arm_2 也跟着改变，而且 arm_2 的位置又受到 arm_1 段的影响，更甚至今天机器人在离桌子更远的地方，那麽要机器人拿取桌上的物体就必须计算出更多座标系之间的资讯。

回传 Gazebo 中机器人的座标资讯

当然，今天我们的主题并没有那麽复杂，主要还是着重在於得到机器人在 Gazebo 当中的座标资讯，其实也不用担心太复杂，因为 ROS 本身已经有一套相当好用的 Package 可以使用啦，马上就来介绍一下吧。

tf 函式库

tf 是 ROS 系统中内建的一个 Package，其功能在於可以让使用者快速地得到两个部件之间其座标的相对资讯，甚至可以运算出过去的时间点中某个部件对应的空间资讯。由於一个机器人常常是由复数个部件所构成，因此 tf 函式库的出现可说是替开发者节省了很多时间。以下我们先透过几个简单的 tf 指令来了解一下其功用：

基本 tf 指令

首先，先开启任意一个带有 waffle 的 Gazebo 世界：

roslaunch turtlebot3_gazebo turtlebot3_world.launch

接着可以透过以下指令来看当前座标系之间的关系：

rosrun tf view_frames

看到 Terminal 上跳出几行讯息後，此时便会生成一份叫做 frames.pdf 的档案如下图：

在这当中可以看到目前有两个座标系分别为 odom 与 base_footprint，odom 即为 Gazebo 世界中的原点座标；而 base_footprint 便是 waffle 本身的座标。那麽为什麽两者会命名为 odom 与 base_footprint 呢?原因可以从 turtlebot3_waffle.gazebo.xacro 当中的这一段定义中看到：

  <gazebo>
    <plugin name="turtlebot3_waffle_controller" filename="libgazebo_ros_diff_drive.so">
      
      ...
      
      <odometryFrame>odom</odometryFrame>
      
      ...
      
      <robotBaseFrame>base_footprint</robotBaseFrame>
      
      ...
      
    </plugin>
  </gazebo>

有没有觉得 .xacro 档愈来愈神奇阿，可以看到档案中对其预设的命名即为 odom 与 base_footprint。那麽接着我们能再透过以下指令观察到两者之间的相对座标：

rosrun tf tf_echo base_footprint odom

可以看到当前 Terminal 上回传了 Translation、Rotation in Quaternion 与 Rotation in RPY，以下简单个别解释一下：

1. Translation: 位置资讯，即为卡氏座标系的 [x, y, z]
2. Rotation in Quaternion: 旋转资讯，四个参数为四元数表示法 a+bi+cj+dk
3. Rotation in RPY: 旋转资讯，就是曾有有提到的 [roll, pitch, yaw]

根据 Translation 所回馈的数值我们就可以知道这应为 waffle 相对於原点的座标资讯，因此原指令 rosrun tf tf_echo [frame1] [frame2] 表示的是 [frame1] 相对於 [frame2] 的座标资讯。

基本的指令操作就介绍到这边，接着我们进入 tf 程序码的部分。tf 函式库分成 C++ 与 Python 两种语法可做使用，而今天笔者就使用 Python 与一个简单的范例来介绍如何回传机器人在 Gazebo 当中的座标：

程序码

首先，先在 catkin_ws/src/ 下建立一个名为 tf_test.py 的 Package

catkin_create_pkg tf_test rospy

接着建立档案 tf_listener.py，并输入以下程序码：

#!/usr/bin/env python  
import roslib
roslib.load_manifest('tf_test')
import rospy
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('waffle_position')
    listener = tf.TransformListener()

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/odom', '/base_footprint', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        print('x = ',trans[0])
        print('y = ',trans[1])
        rate.sleep()

我们直接来看这段程序码：

        try:
            (trans,rot) = listener.lookupTransform('/odom', '/base_footprint', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

其中，listener.lookupTransform 函式的定义为：

lookupTransform(target_frame, source_frame, time) -> (position, quaternion)

意思是说，执行该函式後将得到 source_frame 相对於 target_frame 的座标资讯，time 表示相对时间，使用 rospy.time(0) 则表示两者当前的相对资讯。该函式会输出成 position 与 quaternion 两个 list，分别储存 Translation (x, y, z) 与 Rotation (x, y, z, w)。
在此范例中，我们欲求得当前 base_footprint 相对於 odom 之座标，并将其输出为 trans 与 rot。

成功建立後，我们继续使用 turtlebot3_world.launch 来进行测试。成功开启世界後，执行前几天在 robot_control/src/ 中建立好的 test.py，接着再回到 tf_test/src/ 下执行 tf_listener.py，就能看到 Terminal 上不断印出当前 waffle 的座标罗。

结语

今天我们介绍大致介绍了座标系的重要性，以及使用两个基础的 tf 指令以及一个基本的 tf 函式，希望大家对於 tf 这项工具有一个初步的认识。明天我们则会开始介绍 rviz 在 Gazebo 当中的应用。