这段时间一直在跟着任乾大佬的文章学习slam定位，发现还是得把ＴＦ重新学习一下，之前学的不扎实．就跟着网上的教程学习，这里记录一下学习的过程和笔记．

这篇关于ＴＦ变换的教程写的真好，推荐先看看这个

官方教程在这里

在网上看了很多教程后，发现ＴＦ最重要的是它相邻两个节点之间只有一个遍历的方法，一个重点，这表名了，在坐标传递的过程中的唯一性．

首先是跟着这篇文档学习的，

学习tf变换前要先学习一个命令，是可视化显示tf_tree的命令

rosrun rqt_tf_tree rqt_tf_tree

 说明是从base_link是frame_id，而base_laser是child_frame_id,然后具体的变换需要根据源码来判断．

一.背景介绍

有一个小车它的坐标系为base_link，小车上有一个激光雷达它的坐标系为base_laser,如下图所示：

红色的部分是base_link

灰色的部分是base_laser

　　　　　左右

激光雷达在在小车的中心位置的右上方具体位移t矩阵tf::Vector3(0.1,0,0.2),以右手坐标系，图片右边的为ｘ轴．

所以说，从base_link坐标系到base_laser坐标系之间的变换就是base_link上的点-(0.1,0,0.2)就得到base_laser坐标系上的点 ，反过来就是在base_laser上的点+(0.1,0,0.2)就得到base_link上的点．

这样只有不多的坐标系转换比较容易理解，但在复杂的工程中设计好几十个变换，理解起来就比较复杂．ros为我们提供了ＴＦ变换，来辅助我们在工程中进行这种复杂的坐标系转换．

以下图为例：

 还是小车+激光雷达模型，激光雷达扫描到前面的一面墙，得到的激光点在激光雷达坐标系base_laser里的坐标是(0.3,0.0,0.0)，通过上面的转换关系，可以得到激光点在小车坐标系base_link里的坐标是(0.4,0.0,0.2).

二．具体发布的例子

通过一个例子来说明：

首先创建一个ros工作空间和相应的包，用到了roscpp tf geometry_msgs等包

mkdir -p catkin_ws/src && cd catkin_ws/src
catkin_create_pkg robot_setup_tf roscpp tf geogmetry_msgs

然后在src下创建tf_broadcaster.cpp，以下为源码：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "tf_publisher");
    ros::NodeHandle nh;
    ros::Rate r(100);

    tf::TransformBroadcaster broadcaster;

    while(nh.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")
        );
        // 这里发送的是从base_laser到base_link下的Ｔ矩阵
        // nh.sleep();
    }

    return 0;

}

这里核心的有两处，

 tf::TransformBroadcaster broadcaster

这个类的来源于头文件#include<tf/transform_broadcaster.h>

目的是为了帮助我们更快捷传送tf数据格式，有点类似于封装了大部分功能的topic通讯中的publish.

broadcaster.sendTransform(tf::StampedTransform(
　　　　tf::Transform(tf::Quatenion(0,0,0,1),
　　　　　　　　　　　tf::Vector3(0.1,0.0,0.2)),
　　　　　　　　　　　ros::Time::now(),
　　　　　　　　　　　"base_link", 
　　　　　　　　　　　"base_laser"));

这里调用了broadcaster的函数sendTransform()这个函数的主要作用:

  /** \brief Send a StampedTransform 
   * The stamped data structure includes frame_id, and time, and parent_id already.  */
  void sendTransform(const StampedTransform & transform);

主要是广播发送StampedTransform类型的数据结构

tf::StampedTransform()函数的主要作用：

  ros::Time stamp_; ///< The timestamp associated with this transform
  std::string frame_id_; ///< The frame_id of the coordinate frame  in which this transform is defined
  std::string child_frame_id_; ///< The frame_id of the coordinate frame this transform defines
  StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id):
    tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ }

包含了各种信息的数据结构，针对源码进行分析．

第一个参数是base_link和base_laser之间的坐标系转换的关系，

第二个参数是时间戳，

第三个参数是parent_frame_id，就是base_link,

第四个参数是child_frame_id,就是base_laser.

源码解释完毕，之后编译运行

注意CMakeLists.txt中需要加入这几行：

add_executable(tf_broadcaster src/tf_broadcaster.cpp)
add_dependencies(tf_broadcaster ${tf_broadcaster_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(tf_broadcaster
  ${catkin_LIBRARIES}
)

一定要加上add_dependencies(．．．)不然会报什么什么main缺失．

然后就catkin_make运行，记得要source 一下

看一下rqt

 只有我们定义的节点/tf_publisher,

在用rostopic echo /tf观察一下，有没有发送

说明tf::TransformBroadcaster 和publish类似．都是发送数据

三．具体接收的例子

还是在刚刚的包中，在src文件下新建tf_listener.cpp文件

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>	

void transformPoint(const tf::TransformListener& listener){
  //创建一个函数，参数为TransformListener,作用为将“base_laser”坐标系的点，变换到“base_link”坐标系中。
  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();
  //消息名字最后的“Stamped”的意义是，它包含了一个头部，
  //允许我们去把时间戳和消息的frame_id相关关联起来。
  //我们将会设置laser_point的时间戳为ros::time(),
  //即是允许我们请求TransformListener取得最新的变换数据。
  

  //创建的点为虚拟点
  laser_point.point.x = 0.3;
  laser_point.point.y = 0.0;
  laser_point.point.z = 0.0;

  try{
    geometry_msgs::PointStamped base_point;
    listener.transformPoint("base_link", laser_point, base_point);
    //我们已经有了从“base_laser”到“base_link”变换的点数据。进一步，我们通过TransformListener对象，
    // 调用transformPoint(),填充三个参数来进行数据变换。第1个参数，代表我们想要变换的目标坐标系的名字。
    // 第2个参数填充需要变换的原始坐标系的点对象，第3个参数填充，目标坐标系的点对象。
    // 所以，在函数调用后，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();

  return 0;
}

里面的注释写的很详细了，具体的看注释就好了

核心函数有：
 

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

来自头文件#include <tf/transform_listener.h>

作用是自动订阅transform信息

/** \brief This class inherits from Transformer and automatically subscribes to ROS transform messages */
class TransformListener : public Transformer { //subscribes to message and automatically stores incoming data

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

这里写的看不大明白又用了bind,又用了多线程，不是重点，以后再研究

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

  /** \brief Transform a Stamped Point Message into the target frame 
   * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */
  void transformPoint(const std::string& target_frame, const geometry_msgs::PointStamped& stamped_in, geometry_msgs::PointStamped& stamped_out) const;

这个函数的主要作用是将第二个参数的数据转换到frame_id是第一个参数的坐标系中，并且将转换后的第一个参数的坐标系中的数据保存到第三个参数中．

有点绕，简单来说就是

第一个参数是一个坐标系，

第二个参数是另一个坐标系中点的数据，

第三个参数是前面那个坐标系中点的数据，

然后函数的作用就是将订阅到之前发送的tf变换数据，应用到这里，将child_frame_id到frame_id变换作用于不同坐标系上的点．

然后运行catkin_make,还要source一下

看一下rqt_graph

 和topic通讯几乎一样，说明就是封装了大部分功能的topic通讯

最后把它们整合一下，写一个launch文件:

<launch>
    <node pkg="robot_set_up" type="tf_broadcaster" name="tf_publisher1" output="screen" />
    <node pkg="robot_set_up" type="tf_listener" name="tf_subscriber1" output="screen" />

</launch>

发现运行的时候总是报错，

 卡了我好久．．．．

最后发现是pkg的名字写错了．．．．

所以一定要细心！！！

 然后就成功运行了．

最后把他们全部添加到launch文件中去，包括rqt_graph, rosrun

<launch>
    <node pkg="robot_setup_tf" type="tf_broadcaster" name="tf_publisher1" output="screen" />
    <node pkg="robot_setup_tf" type="tf_listener" name="tf_subscriber1" output="screen" />

    <!-- 运行tf_tree查看 -->
    <node pkg="rqt_tf_tree" type="rqt_tf_tree" name="rqt_tf_tree" />
    <!-- 运行rqt_graph -->
    <node pkg="rqt_graph" type="rqt_graph" name="rqt_graph" />
</launch>

 最后运行大合影！