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

  • 内容
  • 评论
  • 相关

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

  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 <ros/ros.h>
  2. #include <tf/transform_broadcaster.h>
  3.  
  4. int main(int argc, char** argv){
  5.   ros::init(argc, argv, "robot_tf_publisher");
  6.   ros::NodeHandle n;
  7.  
  8.   ros::Rate r(100);
  9.  
  10.   tf::TransformBroadcaster broadcaster;
  11.  
  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/transform_broadcaster.h>

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

  1. tf::TransformBroadcaster broadcaster;

     创建一个tf::TransformBroadcaster类的实例,用来广播 base_linkbase_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 向量中。第三个参数是时间戳,直接太难过ROSAPI完成。第四个参数是母节点存储的参考系,即base_link,最后一个参数是子节点存储的参考系,即base_laser

4、编写订阅节点

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

  1. #include <ros/ros.h>
  2. #include <geometry_msgs/PointStamped.h>
  3. #include <tf/transform_listener.h>
  4.  
  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";
  9.  
  10.   //we'll just use the most recent transform available for our simple example
  11.   laser_point.header.stamp = ros::Time();
  12.  
  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;
  17.  
  18.   try{
  19.     geometry_msgs::PointStamped base_point;
  20.     listener.transformPoint("base_link", laser_point, base_point);
  21.  
  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. }
  30.  
  31. int main(int argc, char** argv){
  32.   ros::init(argc, argv, "robot_tf_listener");
  33.   ros::NodeHandle n;
  34.  
  35.   tf::TransformListener listener(ros::Duration(10));
  36.  
  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)));
  39.  
  40.   ros::spin();
  41.  
  42. }

   进行详细的分析:

  1. #include <tf/transform_listener.h>

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

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

   我们创建一个回调函数,每次收到tf消息时,都会自动调用该函数,上一节我们设置了发布tf消息的频率是1Hz,所以回调函数执行的频率也是1Hz。在回调函数中,我们需要完成数据从base_laserbase_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";
  4.  
  5.   //we'll just use the most recent transform available for our simple example
  6.   laser_point.header.stamp = ros::Time();
  7.  
  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.00.20.0)。该类型包含标准的header消息结构,这样,我们可以就可以在消息中加入发布数据的时间戳和参考系的id

  1. try{
  2.     geometry_msgs::PointStamped base_point;
  3.     listener.transformPoint("base_link", laser_point, base_point);
  4.  
  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)。

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

 

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

     


原创文章,转载请注明: 转载自古月居

本文链接地址: ROS探索总结(二十二)——设置机器人的tf变换

微信 OR 支付宝 扫描二维码
为本文作者 打个赏
pay_weixinpay_weixin

评论

27条评论
  1. Gravatar 头像

    二饼 回复

    古大神你好,我把底盘驱动节点,雷达节点,还有建图,move_base节点,tf通通启动后,然后用roswtf检查,出现了这个错误。我在网上搜了搜,没找到。您知道这是什么原因吗?
    ERROR The following nodes should be connected but aren't:
    * /move_base->/move_base (/move_base/global_costmap/footprint)
    * /move_base->/move_base (/move_base/local_costmap/footprint)

  2. Gravatar 头像

    二饼 回复

    古大神你好,我把底盘驱动节点,雷达节点,还有建图,move_base节点,tf通通启动后,然后用roswtf检查,出现了这个错误。我在网上搜了搜,没找到。您知道这是什么原因吗?

  3. Gravatar 头像

    特斯莱丝 回复

    您好!
    如果我把LMS100倒置建地图 该怎么设置tf,

      • Gravatar 头像

        王翰 回复

        @古月 参考大神您的CSND上发现还是有问题“yaw是围绕x轴旋转的偏航角,pitch是围绕y轴旋转的俯仰角,roll是围绕z轴旋转的翻滚角” 后来去看官网发现好像是yaw is rotation about Z, pitch is rotation about Y, and roll is rotation about X好像就是绕X旋转1.57,好像是的,现在我在调节里程计的数据时 校准好像是对的 但是做实验就是误差很大, 可有点什么建议吗? 谢谢大神回复

        • 古月

          古月 回复

          @王翰 1. 首先需要校准里程计的平移和旋转数据,这一步非常重要。
          2. 如果是平面移动机器人,只涉及到围绕一个轴的旋转,不会涉及到三轴,数据上可以自己定义。
          3. 1.57就是90度,这里我不太明白你的意思。

          • Gravatar 头像

            王翰 回复

            @古月 1.我有校准的,但是在校准的时候,他会冷不丁的先后退一下,或者快要到1m的时候就后退一下,我的轮子不是很好,又买了一套新轮再做实验看一下。
            2.那个1.57,我说多了,应该是3.14,就是绕X轴旋转180°。
            3.tf的冗余如果设的比较大,会影响里程计的误差吗?我想问一下。

            • 古月

              古月 回复

              @王翰 嗯,先确保机器人硬件上没有问题。
              tf是坐标系的变换关系,是确定的;里程计发布的是里程计消息,也就是需要校准的数据,校准数据本身和tf没多大关系的

  4. Gravatar 头像

    yzj 回复

    古月大神你好,我想问下有什么办法能够获得机器人在地图中的位置,我是用gmapping建图,但是不知道该怎样获得机器人在占据地图中的位置,希望能得到回答,谢谢。

    • 古月

      古月 回复

      @yzj 你好,/map下的机器人坐标就是在地图中的位置,原点在地图yaml中设置,可以参考:http://blog.csdn.net/yngki/article/details/53258645

      • Gravatar 头像

        yzj 回复

        @古月 谢谢古月大神的回答,然后我想问下古月大神有没有做过就是对gmapping输出的map进行修改的,就是我想在map上添加一些信息。

        • 古月

          古月 回复

          @yzj 数据层面上我没做过修改,但是最终生成的地图文件可以用图片编辑器修改

  5. Gravatar 头像

    海漩涡 回复

    你好月哥 static_tf 与 普通tf有什么差别

    本来理解为static_tf即为不可改变的tf,但是使用
    发布static_tf。我再使用另外一个节点去不断的发改变的tf,rostopic echo tf时会看到还是有不断变化的tf数据收到。

    • 古月

      古月 回复

      @海漩涡 我的理解,就是以命令行的形式发布一个tf,本质上和程序里边写的是一样的

  6. Gravatar 头像

    王召东 回复

    你好,今天读了你的这篇关于tf坐标转换的文章,有些地方不太懂,希望您有时间能解答一下。
    第一,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"));这个语句下被定义或者初始化?或者理解为根本不用定义两个坐标系,sendTransform()函数只是用来说明两个坐标系之间的关系的,只要有定义,便存在着个关系?
    第二,listener.transformPoint("base_link",laser_point,base_point);我可不可以这样理解,发布者发布的消息可以被所有的listenenr接收到而只会转化"base_link"这一类的坐标
    刚刚接触ROS,大神不要介意这些渣渣问题。

    • 古月

      古月 回复

      @王召东 你好,
      1. 这里只是一个简单的例程,不用特殊去定义坐标系,tf就是描述两个坐标系的关系,并不关注坐标系本身,一般在实际的应用中,我们会在机器人的模型文件中详细描述坐标系。
      2. tf是一种广播机制,订阅tf消息的节点都可以拿到完整的tf树,然后从中找到自己需要的坐标变换关系

  7. Gravatar 头像

    李梓杰 回复

    hello,我加了qq群怎么有验证信息呢

  8. Gravatar 头像

    衣言衣语 回复

    你好,我把ros加载到Qt里,想加一个ui界面,在上面添加按钮,以此来启动runlaunch,,,不知道你有没有做过这方面的研究,求赐教!

    • 古月

      古月 回复

      @衣言衣语 你好,这个我没有做过,你可以搜一下,有很多在qt环境下开发ros的案例。

      • Gravatar 头像

        衣言衣语 回复

        @古月 你好,是这样的,我在网上搜到的最多的就是这篇文章http://www.cnblogs.com/cslxiao/p/5125620.html,,但是并没有介绍如何添加ui界面,而且至今也没有找到如何添加ui的文章,所以一直是一头雾水呢

        • 古月

          古月 回复

          @衣言衣语 qt我并不了解,是否可以在ui界面按键的回调函数中启动一些系统指令,这样就可以使用roslaunch命令了。

  9. Gravatar 头像

    belion 回复

    请问,基于ROS开发机器人,硬件平台一般是什么?

    • 古月

      古月 回复

      @belion 这里是官方支持的机器人平台:http://wiki.ros.org/Robots
      自己设计的机器人同样可以适用ROS,硬件平台非常灵活

  10. Gravatar 头像

    __招谁惹谁 回复

    Hello ,How to use python3 in ros?sorry, I have no chinese input.

发表评论

电子邮件地址不会被公开。 必填项已用*标注