C++ tf::TransformListener

用ROS编写机器人的时候，不免会用到坐标变换，而TF是ROS中建立坐标系，并且使用各个坐标间转换关系的一个很好的工具。

TF库的目的是实现系统中任一个点在所有坐标系之间的坐标变换，也就是说，只要给定一个坐标系下的一个点的坐标，就能获得这个点在其他坐标系下的坐标。

使用TF功能包主要有以下两种方式：

A：监听tf变换，接收并缓存系统中发布的所有参考系变换，并从中查询所需要的参考系变换。

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

数据结构，基本的数据类型有：Quaternion, vector, point, pose, transform

其中，quaternion表示四元数，vector3是一个3*1的向量，point表示一个点的坐标，Pose是位姿（包括坐标及方向），transform是一个转换的模板

通过监听tf，我们可以避免繁琐的旋转矩阵的计算，而直接获取我们需要的相关信息。
在监听中最常用以下几个函数：
（1）lookupTransform（）；
（2）transformPoint（）；
（3）transformLaserScanToPointCloud（）；

1、lookupTransform（）

1.函数功能：可以过得两个坐标系之间转换的关系，包括旋转与平移。
2.使用例程：http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28C++%29
3.主要步骤：
（1）定义监听器；

tf::TransformListener listener

（2）定义存放变换关系的变量

tf::StampedTransform transform;

（3）监听两个坐标系之间的变换

try{
      listener.lookupTransform("/turtle2", "/turtle1",
                               ros::Time(0), transform);
}
catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
}

（4）使用：

transform.getOrigin().getX()；
transform.getRotation().getX()；

注意：
A.由于tf的会把监听的内容存放到一个缓存中，然后再读取相关的内容，而这个过程可能会有几毫秒的延迟，也就是，tf的监听器并不能监听到“现在”的变换，所以如果不使用try,catch函数会导致报错：

“world” passed to lookupTransform argument target_frame does not exist. ”

并且会导致程序挂掉，使用try,catch之后就OK了。

程序中使用tf变换比较容易，只需要以下几步：
1、添加头文件：

#include <tf/transform_listener.h>

2、定义一个listener

tf::TransformListener tf_listener;

这个可以写在public中

3、定义存放变换关系的变量

tf::StampedTransform transform;

4、监听两个坐标系之间的变换

try{
      listener.lookupTransform("/map", "/base_link",
                               ros::Time(0), transform);
}
catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
}

这里监听了base_link到map的坐标变换，并将参数存放到transform中。

另外，注意到ros::Time(0)的使用是获取了某一个tf变换中最后一次的数据，注意是最后一次，不是当前时间的。所以对于某些tf时间性要求比较高的场合，比如物体时别。当我从某个位置时别到物体后，发布一个相对于物体的TF。然后我再次运动，时别物体，此时假设由于光线等因素影响其实可能我的算法没有时别出来物体，但是使用ros::Time(0)仍然是可以获取到相对变换的，这个变换就是最后一次时别到物体时的相对TF，显然这样子的结果可能会是错误的。

那如果我们需要得到当前时间戳下的TF而不是很早之前的TF应该怎么做？很简单的方式就是增加一个waitfortransform：
将上面的：

listener.lookupTransform("/map", "/base_link",ros::Time(0), transform);

改成

ros::Time now = ros::Time::now();
listener.waitForTransform("/turtle2", "/turtle1",now, ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1",now, transform);

waitForTransform与ros::Time::now()连用可以获取当前最新时间戳下的TF变换，ros::Duration(3.0)代表你能接受的最大时间戳差异，3就是当前时间戳3秒内的数据。因为时间戳完全对齐基本是不可能的，所以一般这里会设置一下给定一个范围。如果获取到的TF不是当前时间戳的一定容许范围内的，数据会被舍弃，这样子下面的lookupTransform就不会执行。

2、transformPoint（）

这个在传感器数据的坐标变换中使用的比较多，用来将一个传感器的数据从一个坐标系转换到另外一个坐标系下。

listener_.transformPoint(“map”,laser_pose,map_pose)；

（1）其中laser_pose,world_pose的数据类型都是 geometry_msgs::PointStamped， 需要定义laser_pose.header.frame.id即该点所属的坐标系（比如激光坐标系）
（2）”map“是指，我要将aser_pose转换到map坐标系下，map_pose是转换的结果。
使用例程

geometry_msgs::PointStamped turtle1;
turtle1.header.stamp=ros::Time();
turtle1.header.frame_id="turtle1";
turtle1.point.x=1;
turtle1.point.y=2;
turtle1.point.z=3;
geometry_msgs::PointStamped turtle1_world;
        try{
listener_.transformPoint("PTAM_world",turtle1,turtle1_world);
         }
             catch (tf::TransformException &ex) {
          ROS_ERROR("%s",ex.what());
          ros::Duration(1.0).sleep();
    }

这里将turtle1从turtle1坐标系转换到PTAM_world坐标系下，数据存储在turtle1_world下。

3、transformLaserScanToPointCloud（）

这个TF一般用于激光SLAM中对于激光点云转换到其他坐标系下，例如将来自激光的数据转换到map下，使用transformPoint也可以转但是会比较麻烦，使用transformLaserScanToPointCloud可以一次性转换。

使用例程

tf::TransformListener tf_listener; 
laser_geometry::LaserProjection projector_;
sensor_msgs::PointCloud mapcloud; 
void XXX::ScanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
    if(!tf_listener.waitForTransform(
            scan_in->header.frame_id,
            "/map",
            scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment),
           ros::Duration(1))){
        ROS_INFO("timestamp error");
        return;
    }   
    try
    {
        projector_.transformLaserScanToPointCloud("/map",*scan_in,mapcloud,tf_listener);
    }
    catch(const std::exception& e)
    {
        ROS_ERROR("%s", e.what());
    }	
	//点云处理：	    
}

最前面定义的三个参数一般定义在class类成员变量中，定义到public下。

注意回调函数中的tf_listener.waitForTransform是有必要加的，要不然会报如下错误：