本系列文章由 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 packagesfind_package(catkin REQUIRED COMPONENTSroscpprospytfturtlesim)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 noderos::init(argc, argv, "my_tf_listener");// 创建 node 的 handleros::NodeHandle node;// 等待 /spawn service ros::service::waitForService("spawn");// 这里为 spawn service 创建了一个 clientros::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 messageros::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 COMPONENTSroscpprospytfturtlesim
  )   
  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 檔是什麼？