ROS机器人的tf变换

1. ROS的TF功能包

TF功能包，可以通过广播TF变换和监听TF变换获取如下坐标变换关系：

• 机器人局部坐标系相对于全局坐标系的关系。
• 机器人夹取的物体相对于机器人中心坐标系的位置.

• 机器人中心坐标系相对于全局坐标系的位置。

  
  坐标系.png

2. TF坐标变换

已知激光雷达和机器人底盘的坐标关系，广播并监听机器人的坐标变换，求解激光雷达数据在底盘坐标系下的坐标值。

exmaple.png

2.1 TF广播器

首先定义TF 广播器(TransformBroadcaster),

// 创建tf的广播器
static tf::TransformBroadcaster br;

接着创建坐标变换值(transform)，比如在这里的变换关系中，没有旋转变换，只有平移变换，所以四元数Quaternion可以为(0,0,0,1)，而位移向量则Vector3是(0.1, 0.0, 0.2)，位移的单位是米。

// 初始化tf数据
tf::Transform transform;
transform.setOrigin( tf::Vector3(0.1, 0.0, 0.2) );
transform.setRotation( tf::Quaternion(0,0,0,1) );

最后发布坐标变换(sendTransform)

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

完整代码如下，

/**
 * 该例程产生tf数据，并计算、发布base_laser的位置指令
 */

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


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

    // 订阅base_link的位置话题
    ros::NodeHandle node;

    // 创建tf的广播器
    static tf::TransformBroadcaster br;

    while(node.ok()){
        // 初始化tf数据
        tf::Transform transform;
        transform.setOrigin( tf::Vector3(0.1, 0.0, 0.2) );
        transform.setRotation( tf::Quaternion(0,0,0,1) );

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

2.2 TF监听器

首先定义一个TF监听器(TransformListener)

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

获取TF变换后进行坐标系变化，此时我们可以设置检测到的base_laser(laser_point)为(0.3, 0.0, 0.0)，然后就该检测信息，通过transformPoint转换到base_link(base_point)上的点。

geometry_msgs::PointStamped base_point;
listener.transformPoint("base_link", laser_point, base_point);

完成代码如下：

/**
 * 该例程监听tf数据，并计算、发布base_laser的位置指令
 */

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PointStamped.h>

int main(int argc, char** argv){

    // 初始化ROS节点
    ros::init(argc, argv, "robot_tf_listener");

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

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

    ros::Rate rate(10.0);
    while (node.ok()){

        //我们将在base_laser帧中创建一个要转换为base_link帧的点
        geometry_msgs::PointStamped laser_point;
        laser_point.header.frame_id = "base_laser";
        
        //我们将在我们的简单示例中使用最近可用的转换
        laser_point.header.stamp = ros::Time();
        
        //laser_point检测点获取
        laser_point.point.x = 0.3;
        laser_point.point.y = 0.0;
        laser_point.point.z = 0.0;
        
        try{
            // 等待获取监听信息base_link和base_laser
            listener.waitForTransform("base_link", "base_laser", ros::Time(0), ros::Duration(3.0));
    
            geometry_msgs::PointStamped base_point;
            listener.transformPoint("base_link", laser_point, base_point);

            ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
                laser_point.point.x, laser_point.point.y, laser_point.point.z,
                base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
        }
        catch(tf::TransformException& ex){
            ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
        }

        rate.sleep();
    }

    return 0;
};

3. 运行结果

启动两个节点时，就可以在监听处获得base_laser和base_link的坐标关系了。
说明的是检测物体为世界坐标系，base_laser和base_link是局部坐标系。

[ INFO] [1565335443.030506273]: base_laser: (0.30, 0.00. 0.00) -----> base_link: (0.40, 0.00, 0.20) at time 1565335443.00