ROS TF原理和使用方法

ROS TF介绍

一、TF是什么？
1.TF是ROS的一个包（package）
2.TF能让用户随时记录各种坐标系之间的变换关系
3.TF能让用户在一个坐标系中进行坐标运算，并将转换关系后的位置关系传到另一个坐标系

在ROS里面查看tf变换关系

rosrun rqt_tf_tree rqt_tf_tree

图中为某一时刻的tf变换图（先要启动一个机器人模型），表示turtle1和turtle2与world之间的关系

也能通过rviz查看，但看上去不太清晰

rosrun rviz rviz

打开rviz后，点击右下方的Add，加入TF

rosrun tf view_frames

会产生一个pdf文件，打开后会有所有的tf变换结果
4.

针对某一个topic查看tf变换关系

rostopic echo /tf -turtle1

二、TF变换怎么使用
以官网的TF变换为例作一部分介绍，图中需要变换的有两方面，一个是机器人本体（base_link)与激光雷达（base_laser)的TF变换，另一个是激光雷达产生的激光点变换到base_link坐标系下（原来产生的激光点不会因为雷达位置变换而同步移动到相应位置，因此需要修改）。
注：红色的为base_link,蓝色的为base_laser

1.base_link与base_laser之间的变换
（原理描述）
如果以base_link作为基准，那么base_laser相对于base_link的水平方向的距离为0.1m，竖直方向的距离为0.2m.因此,从base_laser到base_link的坐标转换关系应该为(x:0.1m,y:0.0m, z:0.2m),相反的转换即是(x:-0.1m,y:0.0m,z:0.2m)。问题来了，如果每一个元件都按这样的方式去转换，当项目庞大，组件繁多的时候，就会应付不过来，因此TF为我们提供了更好的机制。
利用tf来管理这种关系，我们需要把他们添加到转换树（transform tree）中。一方面来说，转换树中的每一个节点都对应着一类坐标系，节点之间的连线即是两个坐标相互转换的一种表示，一种从当前节点到子节点的转换表示。Tf利用树结构的方式，保证了两个坐标系之间的只存在单一的转换，同时假设节点之间的连线指向是从parent到child。

基于我们简单的例子，我们需要创建两个节点，一个“base_link”，一个是“base_laser”。为了定义两者的关系，首先，我们需要决定谁是parent，谁是child。时刻记得，由于tf假设所有的转换都是从parent到child的，因此谁是parent是有差别的。我们选择“base_link”坐标系作为parent，其他的传感器等，都是作为“器件”被添加进robot的，对于“base_link”和“base_laser”他们来说，是最适合的。这就意味着转换关系的表达式应该是(x:0.1m,y0.0m,z:0.2m)。关系的建立，在收到“base_laser”的数据到“base_link”的转换过程，就可以是简单的调用tf库即可完成。我们的机器人，就可以利用这些信息，在“base_link”坐标系中，就可以推理出传感器扫描出的数据，并可安全的规划路径和避障等工作。
（具体实现）
先创建一个用于转换的package，并添加依赖（包名为robot_setup_tf,依赖roscpp,tf,geometry_msgs)

cd %TOP_DIR_YOUR_CATKIN_WS%/src
catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

我们已经创建了package。接着我们需要创建对应的节点，来实现广播任务base_laser->base_link。在robot_setup_tf包中，src目录下面新建一个tf_broadcaster.cpp文件，然后将下面代码贴进去。

cd robot_setup_tf/src
touch tf_broadcaster.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>//包含tf下面的广播tf变换头文件

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_publisher");//初始化robot_tf_publisher节点
  ros::NodeHandle n;	//在robot_tf_publisher节点下面新建一个节点控制器n

  ros::Rate r(100);		//设置节点控制器n的发布频率为100

  tf::TransformBroadcaster broadcaster; //创建tf变换的一个对象（tf类下面的广播的对象，如果不懂，加强c++学习）

  while(n.ok()){				//循环判断节点控制器是否计满100，若是则执行
    broadcaster.sendTransform( //tf创建的对象发布消息
      tf::StampedTransform(		  
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser")); 
        //发布的消息有5个元素，
        //第一个元素tf::Quaternion为旋转变换，就是说base_laser在转换到base_link的时候
        //是否需要进行一定的旋转，
        //第二个元素，tf::Vector3为x,y,z三轴的位移变换，
        //第三个元素为变换的时间（因为变换是根据一定时间执行一次），
        //第四个元素为转换的基准目标，也就是转换后到这个坐标系，
        //第五个元素为待转换的坐标系
    n.sleep();//没有操作的时候，节点控制器休眠
  }
}

2.base_laser的激光点到base_link的变换
在robot_setup_tf功能包中，在src目录下创建tf_listener.cpp，并将下面的代码粘贴到里面：
特别提醒：一个TransformListener对象会自动的订阅ROS系统中的变换消息主题，同时管理所有的该通道上的变换数据。也就是说不需要自己手动添加tf的订阅关系

cd robot_setup_tf/src
touch tf_listener.cpp

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>	//添加tf订阅的头文件

void transformPoint(const tf::TransformListener& listener){
  //创建一个函数，参数为TransformListener,作用为将“base_laser”坐标系的点，变换到“base_link”坐标系中。
  geometry_msgs::PointStamped laser_point;
  laser_point.header.frame_id = "base_laser";

  //we'll just use the most recent transform available for our simple example
  laser_point.header.stamp = ros::Time();
  //消息名字最后的“Stamped”的意义是，它包含了一个头部，
  //允许我们去把时间戳和消息的frame_id相关关联起来。
  //我们将会设置laser_point的时间戳为ros::time(),
  //即是允许我们请求TransformListener取得最新的变换数据。
  

  //创建的点为虚拟点
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

  try{
    geometry_msgs::PointStamped base_point;
    listener.transformPoint("base_link", laser_point, base_point);
    //我们已经有了从“base_laser”到“base_link”变换的点数据。进一步，我们通过TransformListener对象，调用transformPoint(),填充三个参数来进行数据变换。第1个参数，代表我们想要变换的目标坐标系的名字。第2个参数填充需要变换的原始坐标系的点对象，第3个参数填充，目标坐标系的点对象。所以，在函数调用后，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());
  }
}

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_listener");
  ros::NodeHandle n;

  tf::TransformListener listener(ros::Duration(10));

  //we'll transform a point once every second
  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

  ros::spin();

}

3.修改CMakeList.txt

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

4.编译通过

$ cd %TOP_DIR_YOUR_CATKIN_WS%
$ catkin_make

5.运行代码

roslaunch robot_setup_tf tf_broadcaster
rosrun robot_setup_tf tf_listener