1、什么是tf变换

        ROS中的很多软件包都需要机器人发布tf变换树,那么什么是tf变换树呢?抽象的来讲,一棵tf变换树定义了不同坐标系之间的平移与旋转变换关系。具体来说,我们假设有一个机器人,包括一个机器人移动平台和一个安装在平台之上的激光雷达,以这个机器人为例,定义两个坐标系,一个坐标系以机器人移动平台的中心为原点,称为base_link参考系,另一个坐标系以激光雷达的中心为原点,称为base_laser参考系。

image

        假设在机器人运行过程中,激光雷达可以采集到距离前方障碍物的数据,这些数据当然是以激光雷达为原点的测量值,换句话说,也就是base_laser参考系下的测量值。现在,如果我们想使用这些数据帮助机器人完成避障功能,当然,由于激光雷达在机器人之上,直接使用这些数据不会产生太大的问题,但是激光雷达并不在机器人的中心之上,在极度要求较高的系统中,会始终存在一个雷达与机器人中心的偏差值。这个时候,如果我们采用一种坐标变换,将及激光数据从base_laser参考系变换到base_link参考下,问题不就解决了么。这里我们就需要定义这两个坐标系之间的变换关系。

         为了定义这个变换关系,假设我们已知激光雷达安装的位置在机器人的中心点上方20cm,前方10cm处。这就根据这些数据,就足以定义这两个参考系之间的变换关系:当我们获取激光数据后,采用(x: -0.1m, y: 0.0m, z: -0.2m)的坐标变换,就可以将数据从base_laser参考系变换到base_link参考系了。当然,如果要方向变化,采用(x: 0.1m, y: 0.0m, z: 0.20m)的变换即可。

        从上边的示例看来,参考系之间的坐标变换好像并不复杂,但是在复杂的系统中,存在的参考系可能远远大于两个,如果我们都使用这种手动的方式进行变换,估计很快你就会被繁杂的坐标关系搞蒙了。ROS提供的tf变换就是为解决这个问题而生的,tf功能包提供了存储、计算不同数据在不同参考系之间变换的功能,我们只需要告诉tf树这些参考系之间的变换公式即可,这颗tf树就可以树的数据结构,管理我们所需要的参考系变换。

image

       还是以上边的示例为基础,为了定义和存储base_linkbase_laser两个参考系之间的关系,我们需要将他们添加到tf树中。从树的概念上来讲,tf树中的每个节点都对应一个参考系,而节点之间的边对应于参考系之间的变换关系。tf就是使用这样的树结构,保证每两个参考系之间只有一种遍历方式,而且所有变换关系,都是母节点到子节点的变换。

        为了定义上边示例中的参考系,我们需要定义两个节点,一个对应于base_link参考系,一个对应于base_laser参考系。为了创建两个节点之间的边,我们首先需要决定哪一个节点作为母节点,哪一个节点作为子节点,这一点在tf树中是非常重要的。这里我们选择base_link作为母节点,这样会方便后边为机器人添加更多的传感器作为子节点。所以,根据之前的分析,从base_link节点到base_laser节点的变换关系为(x: 0.1m, y: 0.0m, z: 0.2m)。设置完毕后,我们就可以通过调用tf库,直接完成base_laser参考系到base_link参考系的数据坐标变换了。

2、代码流程

       通过上边的分析,应该可以从理论上帮助你理解什么是tf,以及tf的功能了。在实际应用中,我们需要使用代码来完成这些理论,下边我们就来看看如何使用代码来调用tf的变换。

       代码的总体思路分为两个部分:

       1)编写一个节点,广播两个参考系之间的tf变换关系

       2)编写另外一个节点,订阅tf树,然后从tf树中遍历到两个参考系之间的变换公式,然后通过公式计算数据的变换。

        我们先来完成第一步。首先我们来创建一个功能包,用于后边程序的放置,这里需要依赖roscpptfgeometry_msgs

$ cd %TOP_DIR_YOUR_CATKIN_WS%/src
$ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

3、编写广播节点      

       进入功能包,创建src/tf_broadcaster.cpp文件,来完成广播节点的代码:

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

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

  ros::Rate r(100);

  tf::TransformBroadcaster broadcaster;

  while(n.ok()){
    broadcaster.sendTransform(
      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"));
    r.sleep();
  }
}

   逐行分析如上代码:

#include <tf/transform_broadcaster.h>

        因为后边会使用到tf::TransformBroadcaster类的实例,来完成tf树的广播,所以需要先包含相关的头文件。

tf::TransformBroadcaster broadcaster;

     创建一个tf::TransformBroadcaster类的实例,用来广播 base_linkbase_laser的变换关系。

   broadcaster.sendTransform(
      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"));

    这部分是代码的关键所在。通过TransformBroadcaster 类来发布变换关系的接口,需要五个参数。首先是两个参考系之间的旋转变换,通过btQuaternion四元数来存储旋转变换的参数,因为我们用到的两个参考系没有发生旋转变换,所以倾斜角、滚动角、偏航角都是0。第二个参数是坐标的位移变换,我们用到的两个参考系在X轴和Z轴发生了位置,根据位移值填入到btVector3 向量中。第三个参数是时间戳,直接太难过ROSAPI完成。第四个参数是母节点存储的参考系,即base_link,最后一个参数是子节点存储的参考系,即base_laser

4、编写订阅节点

        上一节讲解了如何使用代码编写一个广播tf变换的节点,这一节,我们编写一个订阅tf广播的节点,并且使用tf树中base_linkbase_laser的变换关系,完成数据的坐标变换。在robot_setup_tf功能包中创建src/tf_listener.cpp文件,代码如下:

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

void transformPoint(const tf::TransformListener& listener){
  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  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();

  //just an arbitrary point in space
  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);

    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();

}

   进行详细的分析:

#include <tf/transform_listener.h>

   在后边的代码中,我们会使用到tf::TransformListener对象,该对象会自动订阅ROS中的tf消息,并且管理所有的变换关系数据。所以需要先包含相关的头文件。

void transformPoint(const tf::TransformListener& listener){

   我们创建一个回调函数,每次收到tf消息时,都会自动调用该函数,上一节我们设置了发布tf消息的频率是1Hz,所以回调函数执行的频率也是1Hz。在回调函数中,我们需要完成数据从base_laserbase_link参考系的坐标变换。

//we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  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();

  //just an arbitrary point in space
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

   我们创建了一个geometry_msgs::PointStamped类型的虚拟点,该点的坐标为(1.00.20.0)。该类型包含标准的header消息结构,这样,我们可以就可以在消息中加入发布数据的时间戳和参考系的id

try{
    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());
  }

   这里是代码的关键位置。我们已经在base_laser参考系下虚拟了一个数据点,那么怎样将该点的数据转换到base_base参考系下呢?使用TransformListener 对象中的transformPoint()函数即可,该函数包含三个参数:第一个参数是需要转换到的参考系id,当然是base_link了;第二个参数是需要转换的原始数据;第三个参数用来存储转换完成的数据。该函数执行完毕后,base_point就是我们转换完成的点坐标了!

catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  }

   为了保证代码的稳定性,我们也需要应对出错处理,例如当tf并没有发布需要的变换关系时,在执行transformPoint时就会出现错误。

5、编译代码

     到此为止,我们已经完成了代码编写的全部流程,接下来就是编译代码了!打开功能包的CMakeLists.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})

     然后保存文件,经行编译:

$ cd %TOP_DIR_YOUR_CATKIN_WS%
$ catkin_make

6、运行代码

     终于来到最激动人心的一步,见证奇迹的时刻就要来临!

     打开一个终端,运行roscore

roscore

     再打开一个终端,运行tf广播tf_broadcaster

rosrun robot_setup_tf tf_broadcaster

     还要再打开一个终端,运行tf监听tf_listener,将我们虚拟的点坐标,从base_laser转换到base_link参考系下:

rosrun robot_setup_tf tf_listener

     如果一切正常,你应该已经可以在第三个终端中看到如下的数据了:

[ INFO] 1248138528.200823000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138528.19

[ INFO] 1248138529.200820000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138529.19

[ INFO] 1248138530.200821000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138530.19

[ INFO] 1248138531.200835000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138531.19

[ INFO] 1248138532.200849000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138532.19

     从上边的信息中,我们可以清晰的看到,我们虚拟的坐标点,成功转换到了base_link参考系下,坐标位置变换成了(1.10, 0.20, 0.20)。

     到此为止,我们已经完成了例程的所有讲解,在你需要完整的真实系统中,请根据需求将PointStamped数据类型修改成你所使用传感器发布的实际消息对应类型。

 

参考链接:http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF