10、tf发布与监听

10.1、tf功能包

10.1.1、tf是一个让用户随时间跟踪多个坐标系的功能包，它使用树形数据结构，根据时间缓冲并维护多个坐标系之间的坐标变换关系，可以帮助开发者在任意时间、坐标系间完成点、向量等坐标变换。

10.1.2、使用步骤

1）、监听tf变换

接收并缓存系统中发布的所有坐标系变换数据，并从中查询所需要的坐标变换关系。

2）、广播tf变换

向系统中广播坐标系之间的坐标变换关系。系统中可能会存在多个不同部分的tf变换广播。每个广播都可以直接将坐标变换关系插入tf树中，不需要再进行同步。

10.2、tf坐标系广播与监听的编程实现

10.2.1、创建并且编译功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_tf rospy roscpp turtlesim tf
cd ..
catkin_make

10.2.2、如何实现一个tf广播器

1）、定义tf广播器（TransformBroadcaster）；

2）、初始化tf数据，创建坐标变换值；

3）、发布坐标变换（sendTransform）；

10.2.3、如何实现一个tf监听器

1）、定义TF监听器（TransformListener）；

2）、查找坐标变换（waitForTransform、lookupTransform）

10.2.4、C++语言实现tf广播器

1）、在功能包learning_tf的src文件夹,创建一个c++文件（文件后缀为.cpp），命名为turtle_tf_broadcaster.cpp

2）、把下边的程序代码复制粘贴到turtle_tf_broadcaster.cpp文件中

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

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    
    static tf::TransformBroadcaster br;// 创建tf的广播器

    // 初始化tf数据
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );//设置xyz坐标
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);//设置欧拉角：以x轴，y轴，z轴旋转
    transform.setRotation(q);

    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));// 广播world与turtle坐标系之间的tf数据
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "turtle_world_tf_broadcaster");// 初始化ROS节点

    if (argc != 2)
    {
        ROS_ERROR("Missing a parameter as the name of the turtle!"); 
        return -1;
    }
    
    turtle_name = argv[1];// 输入参数作为海龟的名字

    // 订阅海龟的位姿话题/pose
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

        // 循环等待回调函数
    ros::spin();

    return 0;
};

3）、程序流程图

4）、代码解析

首先，先订阅小海龟的/pose位姿话题，倘若有该话题发布，那么就进入回调函数。再回调函数里边，首先先创建了tf的广播器，然后初始化tf数据，数据的值就是订阅/pose话题传过来的数据。最后，通过br.sendTransform把小海龟对于世界坐标的变换发布出去，这里说下sendTransform这个函数。有4个参数，第一个参数表示tf::Transform类型的坐标转换（也就是之前初始化的tf数据），第二个参数是时间戳，第三个和第四个是变换的源坐标系和目标坐标系。

10.2.4、C++语言实现tf监听器

1）、在功能包learning_tf的src文件夹,创建一个c++文件（文件后缀为.cpp），命名为turtle_tf_listener.cpp

2）、把下边的程序代码复制粘贴到turtle_tf_listener.cpp文件中

xxxxxxxxxx
/**
 * 该例程监听tf数据，并计算、发布turtle2的速度指令
   turtle2->turtle1 = world->turtle*world->turtle2
 */

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

int main(int argc, char** argv)
{
    
    ros::init(argc, argv, "turtle1_turtle2_listener");// 初始化ROS节点

   
    ros::NodeHandle node; // 创建节点句柄

    // 请求服务产生turtle2
    ros::service::waitForService("/spawn");
    ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    // 创建发布turtle2速度控制指令的发布者
    ros::Publisher vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);

    tf::TransformListener listener;// 创建tf的监听器

    ros::Rate rate(10.0);

    while (node.ok())
    {
        // 获取turtle1与turtle2坐标系之间的tf数据
        tf::StampedTransform transform;
        try
        {
            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;
        }

        // 根据turtle1与turtle2坐标系之间的位置关系，通过数学计算公式，得出角速度和线速度，发布turtle2的速度控制指令
        geometry_msgs::Twist turtle2_vel_msg;

        turtle2_vel_msg.angular.z = 6.0 * atan2(transform.getOrigin().y(),
                                        transform.getOrigin().x());
        turtle2_vel_msg.linear.x = 0.8 * sqrt(pow(transform.getOrigin().x(), 2) +
                                      pow(transform.getOrigin().y(), 2));
        vel.publish(turtle2_vel_msg);

        rate.sleep();
    }
    return 0;
};

3）、程序流程图

4）、代码解析

首先，通过服务调用产生另外一只小乌龟turtle2，然后创建turtle2速度控制发布者；接着创建一个监听器，监听和查找turtle1和tuetle2的左边变换，这里涉及到两个函数waitForTransform和lookupTransform

waitForTransform(target_frame,source_frame,time,timeout):两个frame分别表示目标坐标系和源坐标系，time表示等待两个坐标系之间变换的时间，因为坐标变换是阻塞程序，所以需要设置timeout，表示超时时间。

lookupTransform(target_frame,source_frame,time,transform):给定源坐标系(source_frame)和目标坐标系(target_frame)，得到两个坐标系之间指定时间(time)的坐标变换(transform)。

经过lookupTransform我们得到了坐标变换的结果transform，然后通过transform.getOrigin().y(),transform.getOrigin().x()得到x,y的值，接着通过数学运算得到角速度angular.z和线速度linear.x，最后发布出去，让turtle2做运动。

10.2.5、修改CMakelist.txt和编译

1）、修改修改CMakelist.txt

修改功能包下的CMakelist.txt，添加下边内容

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

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

2）、编译

xxxxxxxxxx
cd ~/catkin_ws
catkin_make
source devel/setup.bash     #需要配置环境变量，否则系统无法找到运行程序

10.2.6、启动与运行效果演示

1）、启动

xxxxxxxxxx
roscore
rosrun turtlesim turtlesim_node
rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
rosrun learning_tf turtle_tf_listener
rosrun turtlesim turtle_teleop_key  #打开海龟键盘控制程序，控制海龟运动

2）、效果展示

3）、程序说明

启动roscore后，开启小海龟节点，这时候终端会出现一只小海龟；然后我们发布两个tf变换，turtle1->world，turtle2->world，因为要想知道turtle2与turtle1之间的变化，则需要知道他们跟world之间的变换；然后，开启tf监听程序，这时候会发现终端产生了另外一只小海龟turtle2，并且turtle2会向turtle1移动；然后，我们打开键盘控制，通过按下方向键来控制turtle1移动，然后turtle2会追着turtle1移动。

10.2.7、Python语言实现tf广播器

1）、在功能包learning_tf,创建一个文件夹script，切换到该目录下，新建一个.py文件，命名为turtle_tf_broadcaster.py

2）、把下边的程序代码复制粘贴到turtle_tf_broadcaster.py文件中

xxxxxxxxxx
#!/usr/bin/env python
# -*- coding: utf-8 -*-

import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    br = tf.TransformBroadcaster()#定义一个tf广播
    #广播world与输入命名的turtle之间的tf变换
    br.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('turtle1_turtle2_tf_broadcaster')#初始化ros节点
    
    turtlename = rospy.get_param('~turtle') #从参数服务器中获取turtle的名字
    #订阅/pose话题数据，也就是turtle的位姿信息
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

3）、程序流程图

10.2.8、Python语言实现tf监听器

1）、在功能包learning_tf的script文件夹,创建一个python文件（文件后缀为.py），命名为turtle_tf_listener.py

2）、把下边的程序代码复制粘贴到turtle_tf_listener.py文件中

xxxxxxxxxx
#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')#初始化ros节点

    listener = tf.TransformListener()#初始化一个监听者

    rospy.wait_for_service('spawn')
    #调用服务产生另一只海龟turtle2
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(8, 6, 0, 'turtle2')
    #声明一个发布者，用来发布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:
            #查找turtle2和turtle1之间的tf变化
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue
        #通过数学计算，算出线速度和角速度，然后发布出去
        angular = 6.0 * math.atan2(trans[1], trans[0])
        linear = 0.8 * 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()

3）、程序流程图

10.2.9、启动与运行效果展示

1）、编写一个launch文件

在功能包目录下，新建一个文件夹launch，切换至launch里边，新建一个launch文件，命名为start_tf_demo_py.launch，把以下内容复制到里边，

xxxxxxxxxx
<launch>

    <!-- 海龟节点-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <!--广播turtle1->world-->
    <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle1" />
    </node>
    <!--广播turtle2->world-->
    <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
      <param name="turtle" type="string" value="turtle2" /> 
    </node>
    <!--监听-->
    <node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" />
    <!--海龟键盘控制节点-->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
</launch>

2）、启动

xxxxxxxxxx
roslaunch learning_tf start_tf_demo_py.launch

程序运行后，鼠标点击运行launch的窗口，按下方向键，turtle2会跟着turtle1移动。

3）、运行结果如下图