TF坐标变换理解和例程

利用海龟例程理解TF变换关系

1. 什么是tf

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

2. tf的使用流程

想要使用tf功能包，总体来讲可以分为以下两个步骤：
（1） 监听tf变换
接收并缓存系统中发布的所有参考系变换，并从中查询所需要的参考系变换。
（2） 广播tf变换
向系统中广播参考系之间的坐标变换关系。系统中更可能会存在多个不同部分的tf变换广播，每个广播都可以直接将参考系变换关系直接插入tf树中，不需要再进行同步。

3. tf海龟例子的实现

1).创建tf广播器
turtle_tf_listener.cpp

#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, "my_tf_listener");

    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 turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    // tf监听器
    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while (node.ok())
    {
        tf::StampedTransform transform;
        try
        {
            // 查找turtle2与turtle1的坐标变换
            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需要运动的线速度和角速度
        // 并发布速度控制指令，使turtle2向turtle1移动
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                        transform.getOrigin().x());
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                      pow(transform.getOrigin().y(), 2));
        turtle_vel.publish(vel_msg);

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

2).创建tf监听器
tf_listener.cpp

#include "ros/ros.h"
#include <iostream>
#include "std_msgs/String.h"
#include <geometry_msgs/PoseArray.h>
#include <sstream>
#include <nav_msgs/OccupancyGrid.h>

//geometry_msgs::PoseArray msgFromMe;
// 接收到订阅的消息后，会进入消息回调函数
void chatterCallback(const geometry_msgs::PoseArray msg)
{
    ROS_INFO("i got it!");
    ROS_INFO("%f",msg.poses[0].position.x);
    ROS_INFO("%f",msg.poses[0].position.y);
    ROS_INFO("%f",msg.poses[0].position.z);
    //ROS_INFO("x= %f",msg.x);
    //msgFromMe.x=msg.x;
    //ROS_INFO("y= %f",msg.y);
    //msgFromMe.y=msg.y;
    //ROS_INFO("z= %f",msg.z);
    //msgFromMe.z=msg.z;
}


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

    // 创建节点句柄
    ros::NodeHandle n;
    ros::Rate loop_rate(10);
    // 创建一个Subscriber，订阅名为chatter的topic，注册回调函数chatterCallback
    ros::Subscriber sub = n.subscribe("chatter", 1000, chatterCallback);
    //ros::Publisher pub = n.advertise<geometry_msgs::Point>("chat", 1000);
    int count=0;
    while(ros::ok())
    {
        // 循环等待回调函数
       // pub.publish(msgFromMe);
        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }

    return 0;
}

3).launch文件
start_demo_listener.launch

 <launch>
    <!-- 海龟仿真器 -->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

    <!-- 键盘控制 -->
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <!-- 两只海龟的tf广播 -->
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle2" name="turtle2_tf_broadcaster" />

    <!-- 监听tf广播，并且控制turtle2移动 -->
    <node pkg="learning_tf" type="turtle_tf_listener"
          name="listener" />

 </launch>

4).运行如上launch文件即可得两只海龟，且利用方向键移动海龟1，海龟2会紧跟着紧跟海龟1。效果图如下：

其tf树如下所示：