ROS 九：坐标管理系统tf简介

关于机器人学中的坐标变换，原本计划在本篇介绍，奈何整理过后的篇幅过长，还烦请移步三维坐标介绍了解相关的前置知识。

一、tf简介

由于机器人上会安装相当多的组件，比如激光雷达、摄像机等等，他们都会组装在机器人外壳上的不同位置，因此他们拥有各自不同的坐标系。

而当一个机器人设计的很庞大的时候，就会有忒儿多的坐标系要我们管理，需要运算相当多的矩阵。

tf功能包会帮助我们管理坐标系，把底层的数学计算都封装好了，我们无需再实现数学计算功能。tf会广播和监听坐标变换，这和我们之前了解到的Publisher与Service这两套通讯方式是不一样的。

tf会记录10秒内机器人所有坐标系的位置关系。所有坐标系都会以树形数据结构存储（tf tree）。

二、tf安装与demo运行

• 安装turtle tf。本教程用的是melodic版本的ROS，需要根据版本号

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

• 运行demo。分别用不同的终端来运行。

roslaunch turtle_tf turtle_tf_demo.launch	#运行launch脚本
rosrun turtlesim turtle_teleop_key			#控制小海龟移动

• 其中，roslaunch是ROS启动launch脚本使用的指令，后续会介绍launch文件的编程

• 启动后，通过turtle_teleop_key控制小海龟移动，可以看到第二只小海龟会比较第一只海龟的坐标，然后会一直跟随第一只小海龟。

2.1 tf的工具

1、view_frames

rosrun tf view_frames				#查看目前tf之间的关系的可视化工具

• tf的view_frames是查看tf之间关系的可视化工具。他会先监听五秒钟，然后保存这些图到一个pdf文件。这个文件会放在终端的当前目录下。

world坐标系是一个全局的坐标系，记录整个仿真器的坐标原点。两个turtle是两个会移动的坐标系。上面的tf tree图就记录了三个坐标系之间的关系。

#rosrun tf tf_echo <node1> ... <node n>
rosrun tf tf_echo turtle1 turtle2

2、tf_echo

tf_echo是一个命令行工具，可以直接帮我们查询几个坐标系之间的关系。他会有类似如下输出：

At time 1659434555.866
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.707, 0.707]
            in RPY (radian) [0.000, -0.000, 1.571]
            in RPY (degree) [0.000, -0.000, 90.000]

• Translation：是指在XYZ坐标上的平移，三个值分别代表X,Y,Z

• Rotation：是指如何旋转会得到一样的姿态。里面三行数据是等价的，知识用三种不同的描述方式展示出来：

  • Quaternion是指四元数，用XYZW表示。
  • RPY是指分别按照x轴，y轴，z轴旋转
    • radian是通过弧度表示
    • degree是通过角度表示

3、rviz

rosrun rviz rviz -d 'rospack find turtle_tf' /rivz/turtle_rivz.rivz

然后要在rviz界面左侧的Fixed Frame选项选择world，在左下角add选项打开菜单，滑到最底下勾选tf，就可以看到前demo正在运行的两只小海龟了。可以试着使用turtle_teleop_key控制小海龟，看rviz的实时效果。

对于两个turtle的坐标关系来说，可以用如下式子计算，是两个4x4矩阵的乘法：
$$
T_{turtle1-turtle2}=T_{turtle1-world}\times T_{world-turtle2}
$$

三、编程实现

1、tf广播器(Broadcaster)

c++

头文件和全局变量

#include<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<turtlesim/Pose.h>

std::string turtle_name;

主函数

int main(int argc,char **argv){
    ros::init(argc,argv,"tf_broadcaster");
    
    //检查输入的参数，并保存下来
    if(argc!=2){
        ROS_ERROR("Need turtle name as argument!");
        return -1;
    }
    turtle_name = argv[1];

    ros::NodeHandle tf_b_node;
    //初始化订阅者
    ros::Subscriber tf_sub = tf_b_node.subscribe(turtle_name+"/pose",10,&TfPoseCallback);
    ros::spin();

    return 0;
}

回调函数：

void TfPoseCallback(const turtlesim::PoseConstPtr& msg)
{
    //创建一个广播器
    static tf::TransformBroadcaster tf_broadcaster;
    
	//设置平移的向量参数
    tf::Transform transform;
    //把平移值放到transform里
    transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0));
    
    //设置旋转四元数
    tf::Quaternion q;
    q.setRPY(0,0,msg->theta);
    //把四元数值放到transform里
    transform.setRotation(q);

   //广播数据，并将数据放在tf tree里
    //函数：StampedTransform(存的变量，时间戳(now)，坐标系1，坐标系2)
    tf_broadcaster.sendTransform(tf::StampedTransform
        (transform,ros::Time::now(),"world",turtle_name));

}

cmake

add_executable(broadcaster src/broadcaster.cpp)
target_link_libraries(broadcaster ${catkin_LIBRARIES})

add_executable(listener src/listener.cpp)
target_link_libraries(listener ${catkin_LIBRARIES})

python

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import roslib
roslib.load_manifest('learn-tf')

import tf
import turtlesim.msg

def handle_turtle_pose(msg,turtlename):
    #定义广播器
    tf_broadcaster = tf.TransformBroadcaster()
    #广播数据
    tf_broadcaster.sendTransform((msg.x,msg.y,0),
        tf.transformations.quaternion_from_euler(0, 0, msg.theta),
        rospy.Time.now(),
        turtlename,
        "world")

if __name__ == '__main__':
    rospy.init_node('tf_broadcaster_py')
    #从超参获取海龟名字
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose'%turtlename,
        turtlesim.msg.Pose,
        handle_turtle_pose,
        turtlename)
    rospy.spin()

2、监听器

c++

头文件

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

主函数

先请求生成turtle2

ros::init(argc,argv,"tf_listener");
ros::NodeHandle tf_l_node;

ros::service::waitForService("/spawn");
ros::ServiceClient add_turtle = tf_l_node.serviceClient<turtlesim::Spawn>("/spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);

再创建发布控制turtle2的发布者

ros::Publisher turtle_vel = tf_l_node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",10);

创建tf的监听器

tf::TransformListener listener;
ros::Rate rate(10.0);

while(tf_l_node.ok()){
    //获取turtle1与turtle2坐标系之间的tf数据
    //保存期平移旋转数据
    tf::StampedTransform transform;
    try
    {
        //等待变换
        //传入变量(两个坐标系、查询的时间戳(当前的时间)、超时的阈值(这里是3s))
    	listener.waitForTransform("/turtle2","turtle1",
			ros::Time(0),ros::Duration(3.0));
        //查询变换
        //传入变量(两个坐标、查询的时间戳(当前的时间)、查询结果存放的变量)
    	listener.lookupTransform("/turtle2","turtle1",
			ros::Time(0),transform);
    }
    catch(tf::TransformException &ex)
    {
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
        continue;
    }
    
    //计算欧氏距离，乘以设置的时间系数
    geometry_msgs::Twist vel_msg;
    // 希望用0.25秒时间完成转动
    // (0.25) * y/x的反正切值
    vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
    	transform.getOrigin().x());
    // 希望用2秒时间完成移动
    // (1/2) * √x^2+y^2
    vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
    	pow(transform.getOrigin().y(), 2));
    //发布turtle2的控制指令
    turtle_vel.publish(vel_msg);

    rate.sleep();
}
return 0;

python

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
import roslib
roslib.load_manifest('learn-tf')

def py_listener():
    rospy.init_node('turtle_tf_listener')
    listener = tf.TransformListener()

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn',turtlesim.srv.Spawn)
    spawner(4,2,0,'turtle2')

    turtle_vel = rospy.Publisher('turtle2/cmd_vel',geometry_msgs.msg.Twist,queue_size=1)
    rate=rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans,rot)=listener.lookupTransform('/turtle2','turtle1',rospy.Time(0))
        except(tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)

        rate.sleep()

if __name__ == '__main__':
    py_listener()

3、运行

在终端运行的方式：

cd ~/ros_ws #进入工作空间根目录
catkin_make
source devel/setup.bash
roscore
rosrun turtlesim turtlesim_node

#下面rosrun的是自己定义功能包的名称和代码
#是自己起名的，配置如下，供参考
#catkin_create_pkg learn-tf roscpp rospy tf turtlesim

# __name的重映射会把我们设置的明智直接取代代码中ros::init设置的节点名
# rosrun learn-tf broadcaster 重映射机制 输入的参数(海龟的名字)
rosrun learn-tf broadcaster __name:=turtle1_broadcaster /turtle1
rosrun learn-tf broadcaster __name:=turtle2_broadcaster /turtle2
rosrun learn-tf listener
rosrun turtlesim turtle_teleop_key

因为这里需要开启很多终端、输入很得多指令，实际启动项目的时候不可能如此繁琐地启动这么多指令。所以下一章将介绍如何使用launch文件快速启动本篇中c++和python的代码。