设置机器人的tf变换

设置机器人的tf变换
转载自古月居：ROS探索总结（二十二）——设置机器人的tf变换

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

假设在机器人运行过程中，激光雷达可以采集到距离前方障碍物的数据，这些数据当然是以激光雷达为原点的测量值，换句话说，也就是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树就可以树的数据结构，管理我们所需要的参考系变换。

还是以上边的示例为基础，为了定义和存储base_link和base_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树中遍历到两个参考系之间的变换公式，然后通过公式计算数据的变换。

我们先来完成第一步。首先我们来创建一个功能包，用于后边程序的放置，这里需要依赖roscpp、tf、geometry_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_link → base_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 向量中。第三个参数是时间戳，直接太难过ROS的API完成。第四个参数是母节点存储的参考系，即base_link，最后一个参数是子节点存储的参考系，即base_laser。

4、编写订阅节点
上一节讲解了如何使用代码编写一个广播tf变换的节点，这一节，我们编写一个订阅tf广播的节点，并且使用tf树中base_link到base_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_laser到base_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.0，0.2，0.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数据类型修改成你所使用传感器发布的实际消息对应类型。