ROS探索总结（二十二）——设置机器人的tf变换

什么是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。

1. $ cd %TOP_DIR_YOUR_CATKIN_WS%/src

2. $ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

3、编写广播节点      

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

1. #include 

2. #include 

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

5.   ros::init(argc, argv, "robot_tf_publisher");

6.   ros::NodeHandle n;

8.   ros::Rate r(100);

10.   tf::TransformBroadcaster broadcaster;

12.   while(n.ok()){

13.     broadcaster.sendTransform(

14.       tf::StampedTransform(

15.         tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),

16.         ros::Time::now(),"base_link", "base_laser"));

17.     r.sleep();

18.   }

19. }

   逐行分析如上代码：

1. #include 

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

1. tf::TransformBroadcaster broadcaster;

     创建一个tf::TransformBroadcaster类的实例，用来广播 base_link → base_laser的变换关系。

1.    broadcaster.sendTransform(

2.       tf::StampedTransform(

3.         tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),

4.         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文件，代码如下：

1. #include 

2. #include 

3. #include 

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

6.   //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame

7.   geometry_msgs::PointStamped laser_point;

8.   laser_point.header.frame_id = "base_laser";

10.   //we'll just use the most recent transform available for our simple example

11.   laser_point.header.stamp = ros::Time();

13.   //just an arbitrary point in space

14.   laser_point.point.x = 1.0;

15.   laser_point.point.y = 0.2;

16.   laser_point.point.z = 0.0;

18.   try{

19.     geometry_msgs::PointStamped base_point;

20.     listener.transformPoint("base_link", laser_point, base_point);

22.     ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",

23.         laser_point.point.x, laser_point.point.y, laser_point.point.z,

24.         base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());

25.   }

26.   catch(tf::TransformException& ex){

27.     ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());

28.   }

29. }

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

32.   ros::init(argc, argv, "robot_tf_listener");

33.   ros::NodeHandle n;

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

37.   //we'll transform a point once every second

38.   ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

40.   ros::spin();

42. }

   进行详细的分析：

1. #include 

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

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

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

1. //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame

2.   geometry_msgs::PointStamped laser_point;

3.   laser_point.header.frame_id = "base_laser";

5.   //we'll just use the most recent transform available for our simple example

6.   laser_point.header.stamp = ros::Time();

8.   //just an arbitrary point in space

9.   laser_point.point.x = 1.0;

10.   laser_point.point.y = 0.2;

11.   laser_point.point.z = 0.0;

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

1. try{

2.     geometry_msgs::PointStamped base_point;

3.     listener.transformPoint("base_link", laser_point, base_point);

5.     ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",

6.         laser_point.point.x, laser_point.point.y, laser_point.point.z,

7.         base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());

8.   }

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

1. catch(tf::TransformException& ex){

2.     ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());

3.   }

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

5、编译代码

     到此为止，我们已经完成了代码编写的全部流程，接下来就是编译代码了！打开功能包的CMakeLists.txt文件，加入相应的编译选项：

1. add_executable(tf_broadcaster src/tf_broadcaster.cpp)

2. add_executable(tf_listener src/tf_listener.cpp)

3. target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})

4. target_link_libraries(tf_listener ${catkin_LIBRARIES})

     然后保存文件，经行编译：

1. $ cd %TOP_DIR_YOUR_CATKIN_WS%

2. $ catkin_make

6、运行代码

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

     打开一个终端，运行roscore：

1. roscore

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

1. rosrun robot_setup_tf tf_broadcaster

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

1. 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）。

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