ROS Indigo 进阶笔记 (二) tf - part 1

本系列文章由 youngpan1101 出品，转载请注明出处。
文章链接： http://blog.csdn.net/youngpan1101/article/details/62230215
作者：宋洋鹏（youngpan1101）
邮箱： yangpeng_song@163.com

---

1. tf/Tutorials

• tf 的官方学习大纲
  
  • Introduction to tf
  • Writing a tf broadcaster 【C++】【Python】
  • Writing a tf listener 【C++】【Python】
  • Adding a frame 【C++】【Python】
  • Learning about tf and time 【C++】【Python】
  • Time travel with tf 【C++】【Python】

2. 官方学习大纲的笔记

2.1 Introduction to tf

• 简介
  该 Demo 描述了三个坐标系：世界坐标系、turtle1 坐标系、turtle2 坐标系，通过$\color{Red}{\rm 坐标系广播（tf \ broadcaster）}$ 发布两 turtle 的坐标系相对世界坐标系的位姿，再使用$\color{Red}{\rm 坐标系监听（tf \ listener）}$两 turtle 坐标系之间的位姿关系，从而计算出其中一只 turtle 跟踪另一只 turtle 所需的运动信息。

• 安装该 Demo 所需要的各种包

  $ sudo apt-get install ros-indigo-ros-tutorials ros-indigo-geometry-tutorials ros-indigo-rviz ros-indigo-rosbash ros-indigo-rqt-tf-tree

• 运行 Demo

  $ roslaunch turtle_tf turtle_tf_demo.launch

• 通过键盘控制其中一只乌龟运动，另一只乌龟会跟着它跑（在 “arrow keys” 窗口获得光标焦点的情况下才能进行键盘控制）
  

• 使用 $\color{Red}{\rm rqt\_tf\_tree}$ 查看 tf 树

  $ rosrun rqt_tf_tree rqt_tf_tree
  或
  $ rqt &       （弹窗菜单栏操作：Plugins >> Visualization >> TF Tree）

  

• 使用 $\color{Red}{\rm view\_frames}$ 将 tf 树状图保存成 pdf

  $ rosrun tf view_frames  （生成 pdf 文档）
  $ evince frames.pdf   （打开 pdf 文档）

• 使用 $\color{Red}{\rm tf\_echo}$ 可以打印出 target_frame 相对 reference_frame 的位姿变换关系

  $ rosrun tf tf_echo [reference_frame] [target_frame]
  [ex]$ rosrun tf tf_echo turtle1 turtle2

  ${\boldsymbol T}_{turtle1\_turtle2} = {\boldsymbol T}_{turtle1\_world} \ {\boldsymbol T}_{world\_turtle2}$
  输出：
  

• 使用 $\color{Red}{\rm rviz}$ 更直观地查看三个坐标系随时间的变换：

  $ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz

  

2.2 Writing a tf broadcaster (C++)

• 简介
  该小节将介绍如何创建一个坐标系广播（tf broadcaster），主要是针对运动中的 turtle 进行位姿变换的广播。

• 创建 learning_tf package

  $ cd  [YOUR_CATKIN_WORKSPACE_HOME]/src
  $ catkin_create_pkg learning_tf tf roscpp rospy turtlesim
  $ cd [YOUR_CATKIN_WORKSPACE_HOME]
  $ catkin_make
  $ source ./devel/setup.bash    

• src/turtle_tf_broadcaster.cpp

   // 包含了最常用的 ROS 系统部分所必须的一些头文件
   #include <ros/ros.h>
   // 包含了用来完成坐标系变换信息发布功能的一些头文件
   #include <tf/transform_broadcaster.h>   
   // 包含了 turtlesim package 中的 /Pose message 头文件
   #include <turtlesim/Pose.h>
   std::string turtle_name;    
   void poseCallback(const turtlesim::PoseConstPtr& msg)
   {
      // 建立用于发布坐标系变换关系的 static 对象
      static tf::TransformBroadcaster br; 
      // 目标坐标系变换矩阵的赋值
      tf::Transform transform;
      transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );  // 设置坐标系的原点
      tf::Quaternion q;  
      q.setRPY(0, 0, msg->theta);  // 欧拉角 >> 四元数
      transform.setRotation(q);    // 四元数 >> 变换矩阵中的旋转矩阵   
      /** 
       *  函数功能：发布目标坐标系信息。
       *  tf::StampedTransform 形参解析：
       *    <arg1>：tf::Transform，即目标坐标系的变换矩阵
       *    <arg2>：使用 ROS 的当前时间給数据打时间戳
       *    <arg3>：目标坐标系的父坐标系的名字
       *    <arg4>：目标坐标系的名字
       **/
      br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
   }   
   int main(int argc, char** argv)
   {
      ros::init(argc, argv, "my_tf_broadcaster"); 
      if (argc != 2)
      {
          ROS_ERROR("need turtle name as argument"); 
          return -1;
      };  
      turtle_name = argv[1];  
      ros::NodeHandle node;   
      /**
       * <arg1>：该 node 订阅的 topic
       * <arg2>：队列的缓存大小，如果队列达到 10 个 messages，若有新的 message，则丢弃旧的 message
       * <arg3>：回调函数地址
       **/
      ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);   
      ros::spin();
      return 0;
   }; 

• CMakeLists.txt

   cmake_minimum_required(VERSION 2.8.3)
   project(learning_tf)
   ## Add support for C++11, supported in ROS Kinetic and newer
   # add_definitions(-std=c++11)
   ## Find catkin macros and libraries
   ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
   ## is used, also find other catkin packages
   find_package(catkin REQUIRED COMPONENTS
     roscpp
     rospy
     tf
     turtlesim
   )
   catkin_package(
      #  INCLUDE_DIRS include
      #  LIBRARIES learning_tf
      #  CATKIN_DEPENDS roscpp rospy tf turtlesim
      #  DEPENDS system_lib
   )
   include_directories(
     ${catkin_INCLUDE_DIRS}
   )
   add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
   target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

• launch/start_demo.launch
  首先启动 turtlesim，也就是小乌龟，然后启动小乌龟的键盘控制程序，有两个参数，线速度和角速度，默认都是2，最后启动我们的广播 nodes，两个小乌龟都会发布自己的坐标系位姿给 world 坐标系。

  <launch>
      <!-- turtlesim_node-->
      <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
      <!-- turtle_teleop_key node -->
      <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>  
      <!-- speed settings -->
      <param name="scale_linear" value="2" type="double"/>
      <param name="scale_angular" value="2" type="double"/>   
      <!-- turtle_tf_broadcaster node -->
      <node pkg="learning_tf" type="turtle_tf_broadcaster"
            args="/turtle1" name="turtle1_tf_broadcaster" />  
      <node pkg="learning_tf" type="turtle_tf_broadcaster"
            args="/turtle2" name="turtle2_tf_broadcaster" />
  </launch>

• 呼叫 node : <node ... />

参数	功能
pkg	该 node 所属的 package
type	该 node 实际的名称，CMakeLists.txt 中 add_executable(node_name src/xx.cpp) 的 node_name
name	也是该 node 的名称，这里可以給该 node 取名字，会将原名覆盖，可以通过 rqt 或 rosnode info 等指令进行查看
args	启动该 node 时的传参， args= “[args_1] [args_2] …” （多个传参通过“空格”来隔开，args_1 对应 main 程序里的 argv[1]）

• 运行 launch

  $ roslaunch learning_tf start_demo.launch （需要关闭之前 turtle_tf_demo.launch）

  可以用 rqt_graph 来查看 nodes：
  

• 检查结果

  $ rosrun tf tf_echo /world /turtle1

  可以用 rqt_tf_tree 来查看 tf 树状图：
  

2.3 Writing a tf listener (C++)

• 简介
  该小节将介绍如何创建一个坐标系监听器（tf listener），主要是监听两只小乌龟的位姿变换，然后计算相对变换关系，最后发布运动量给第二只小乌龟用于跟踪第一只小乌龟。

• src/turtle_tf_listener.cpp

   // 包含了最常用的 ROS 系统部分所必须的一些头文件
   #include <ros/ros.h>
   // 包含了用来完成坐标系变换信息接收功能的一些头文件
   #include <tf/transform_listener.h>
   // 包含了 geometry_msgs package 中的 /Twist message 头文件
   #include <geometry_msgs/Twist.h>
   // 包含了 turtlesim package 中的 /Spawn service 头文件
   #include <turtlesim/Spawn.h>   
   // 主函数
   int main(int argc, char** argv)
   {
     // 初始化 ROS node
     ros::init(argc, argv, "my_tf_listener");
     // 创建 node 的 handle
     ros::NodeHandle node;
     // 等待 /spawn service 
     ros::service::waitForService("spawn");
     // 这里为 spawn service 创建了一个 client
     ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn");
     turtlesim::Spawn srv;
     srv.request.x = 3.0; srv.request.y = 1.0;// 小乌龟的初始位置，弹窗左下角为原点  
     add_turtle.call(srv);  // 调用 turtlesim/Spawn
     // 在 turtle2/cmd_vel topic 上发布 geometry_msgs::Twist message
     ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10); 
     tf::TransformListener listener;  
     ros::Rate rate(10.0);    
     while (node.ok())
     {
        tf::StampedTransform transform;
        try{
        // 等待 turtle1 相对于 turtle2 的坐标系转换，最长等待从现在到 5 秒之后
        listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(5.0));
        // 计算 /turtle1 坐标系相对于 /turtle2 的坐标系变换矩阵，将数据储存在 tf::StampedTransform 对象中   
        // [transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z()] 是 /turtle1 坐标系原点在 /turtle2 坐标系中的坐标表示
        listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
        } catch (tf::TransformException ex){
           ROS_ERROR("%s",ex.what());
           ros::Duration(1.0).sleep();
           continue;
        } 
        geometry_msgs::Twist vel_msg;
        // atan2(y,x) 表示指向 (x,y) 的射线在坐标平面上与 x 轴正方向的夹角（单位：radian）
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), transform.getOrigin().x());
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
        turtle_vel.publish(vel_msg);  
        rate.sleep();
     }
     return 0;
   };

• CMakeLists.txt

  cmake_minimum_required(VERSION 2.8.3)
  project(learning_tf)    
   ## Add support for C++11, supported in ROS Kinetic and newer
   # add_definitions(-std=c++11)  
   ## Find catkin macros and libraries
   ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
   ## is used, also find other catkin packages
  find_package(catkin REQUIRED COMPONENTS
    roscpp
    rospy
    tf
    turtlesim
  )   
  catkin_package(
   #  INCLUDE_DIRS include
   #  LIBRARIES learning_tf
   #  CATKIN_DEPENDS roscpp rospy tf turtlesim
   #  DEPENDS system_lib
  )   
  include_directories(
     ${catkin_INCLUDE_DIRS}
  )   
  add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
  target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})   
  add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
  target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

• build

  $ cd [YOUR_CATKIN_WORKSPACE_PATH]
  $ catkin_make

• launch/start_demo.launch

  <launch>
      <!-- turtlesim_node-->
      <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
      <!-- turtle_teleop_key node -->
      <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>  
      <!-- speed settings -->
      <param name="scale_linear" value="2" type="double"/>
      <param name="scale_angular" value="2" type="double"/>   
      <!-- turtle_tf_broadcaster node -->
      <node pkg="learning_tf" type="turtle_tf_broadcaster"
            args="/turtle1" name="turtle1_tf_broadcaster" />  
      <node pkg="learning_tf" type="turtle_tf_broadcaster"
            args="/turtle2" name="turtle2_tf_broadcaster" />
      <node pkg="learning_tf" type="turtle_tf_listener" name="listener" />              
  </launch>

• 运行 launch

  $ roslaunch learning_tf start_demo.launch 

  可以用 rqt_graph 来查看 nodes：
  
  teleop 是我们键盘控制的 node，它根据按键发布 /turtle1/cmd_vel 消息到 sim（sim是仿真器，就是显示小乌龟的界面那个），sim 会根据信息在界面上移动第一个小乌龟（launch文件中直接启动的那个），sim 会将 turtle1 和 turtle2 的姿态信息发布出去，我们写的广播程序 turtle1_tf_broadcaster 和 turtle2_tf_broadcaster会 订阅 姿态信息并将坐标系信息通过 tf topic 广播出去，发布出来以后 listener 需要这两个坐标系信息，listener 计算这两个信息的相对位置并发布 /turtle2/cmd_vel ,sim 会订阅这个消息，根据此消息在界面上移动第二个小乌龟。（该段引自 tf 理解入门）

---

reference

• tf 理解入门
• ROS 的 tf 包中坐标变换的方法
• ROS Launch 檔是什麼？