ROS学习（十四）--TF坐标变换

       PS:本节内容完全参考机器人操作系统（ROS）及仿真应用，如有侵权，联系速删

         TF（Transform）坐标变换是一种在机器人学和计算机视觉领域常用的技术，用于在不同坐标系之间进行转换和变换操作。TF是ROS（Robot Operating System）中的一个模块，提供了强大的坐标变换功能。

        在TF中，坐标变换是通过维护一个坐标变换树（Coordinate Frame Tree）来实现的。坐标变换树是由多个坐标系组成的层次结构，每个坐标系都有一个唯一的名称，称为Frame ID。树的根节点是一个固定的全局参考坐标系，通常称为"world"或"map"

        使用TF功能包时，包括两个步骤

（1）广播TF变换，向系统中广播坐标系之间的坐标变换关系。系统中可能会存在多个不同部分的TF变换广播，每个广播都可以直接将坐标变换关系插入到TF树中，不需要再进行同步。

（2）监听TF变换，接受并缓存系统中发布的所有坐标变换数据，并从中查询所需要的坐标变换关系。

1. 广播TF变换

首先，需要新建一个 ROS 功能包。在 Ubunlu 里打开一个终端程序，输人如下指令进人ROS 工作空间。

cd ~/catkin_ws/src/

然后输人如下指令新建一个 ROS 功能包。

catkin_create_pkg tf_test_pkg roscpp tf geometry_msgs

这条指令的具体含义

打开 Visual Studio Code，可以看到工作空间里多了一个 tf_test_pkg 文件夹，
在其 src 子文件夹下新建一个名为 tf_broadcaster.cpp 代码文件。其内容如下:

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

int main(int argc, char **argv)
{
    ros::init(argc, argv, "tf_broadcaster");
    ros::NodeHandle n;
    ros::Rate loop_rate(100);

    tf::TransformBroadcaster broadcaster;
    tf::Transform base_link2base_laser;
    base_link2base_laser.setOrigin( tf::Vector3(0.1, 0.0, 0.2) );
    base_link2base_laser.setRotation( tf::Quaternion(0, 0, 0, 1) );

    while(n.ok())
    {
        broadcaster.sendTransform( tf::StampedTransform( base_link2base_laser, ros::Time::now(), "base_link", "base_laser"));
        //broadcaster.sendTransform(
        //    tf::StampedTransform( tf::Transform(tf::Quaternion(0, 0, 0, 0), tf::Vector3(1, 0.0, 0) ),
        //    ros::Time::now(), "base link", "base laser") );
        loop_rate.sleep();
    }
    return 0;
}

代码详细解读： 

(1)代码的开始部分，先 include 了两个头文件，一个是 ROS 的系统头文件;另一个是transfonn_Bradcaster.h头文件。
TF 功能包提供了 TransformBroadcaster 类的实现，以帮助简化TF 发布转换的任务，
要使用 TransformBroadcaster，就需要包含<tf/transfom_Broadcaster.h>头文件。

(2)ROS 节点的主体函数是 int main(int argc，char** argv)，其参数定义和其他 C++语言程序一样。

(3)main()函数里，首先调用 ros::init(argc，argv, “t_broadcaster” )，进行该节点的初始化操作，函数的第三个参数是节点名称。

(4)接下来利用Rate loop_rate(100)控制节点运行的频率，与后面的 looprate.sleep()配合使用。

(5)创建TF 广播器 broadcaster，用于后续发布坐标变换。

(6)创建一个tansform 对象base_link2base_laser，定义存放转换信息(平动，转动的变量)。利用 setOrigin()函数给定平移变换值;
利用setRotation()函数给定旋转变换值，Quaternion()中的前3个参数为 base_laser 子坐标系与 base_link 父坐标系的角度关系，
分别为ro1l(绕x轴)、pitch(绕y轴)、yaw(绕z轴)，最后一个参数为角速度。

(7)为了连续不断地发布坐标变换，使用一个 while (n.ok()) 循环，以 n.ok() 返回值作为循环结束条件，可以让循环在程序关闭时正常退出。

(8)利用sendTransform()函数发布坐标变换到主题"/tf"。

(9)调用 loop_rate.sleep()函数，按之前loop_rate(100)设置的 100Hz将程序挂起。

代码编写完毕，需要将文件名添加到编译文件里才能进行编译。
在 CMakeLists.txt 文件末尾，为 tf_broadcaster.cpp 添加新的编译规则。内容如下:

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

启动一个终端程序，执行如下指令编译工程。
cd ~/catkin ws/
catkin_make

2. 监听TF变换

打开 Visual Studio Code，在工作空间中 tf_test_pkg 文件夹下的 src 子文件夹下新建一个代码文件。
新建的代码文件命名为 tf_listener.cpp。其内容如下。

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

int main(int argc, char **argv)
{
    ros::init(argc, argv, "tf_listener");
    ros::NodeHandle n;
    ros::Rate loop_rate(100);

    tf::TransformListener listener;
    geometry_msgs::PointStamped laser_pos;
    laser_pos.header.frame_id = "base_laser";
    laser_pos.header.stamp    = ros::Time() ;

    laser_pos.point.x = 0.3 ;
    laser_pos.point.y = 0 ;
    laser_pos.point.z = 0 ;

    geometry_msgs::PointStamped base_pos;

    while (n.ok())
    {
        if (listener.waitForTransform("base_link", "base_laser", ros::Time(0), ros::Duration(3)) )
        {
            listener.transformPoint("base_link", laser_pos, base_pos);
            ROS_INFO( "pointpos in base_laser:(%.2f,%.2f,%.2f)", laser_pos.point.x, laser_pos.point.y, laser_pos.point.z);
            ROS_INFO( "pointpos in base_link:(%.2f,%.2f,%.2f)", base_pos.point.x, base_pos.point.y, base_pos.point.z);

            tf::StampedTransform laserTransform;
            listener.lookupTransform( "base_link", "base_laser", ros::Time(0), laserTransform );

            std::cout << "laserTransform.getOrigin().getX():" << laserTransform.getOrigin().getX() << std::endl;
            std::cout << "laserTransform.getOrigin().getY():" << laserTransform.getOrigin().getY() << std::endl;
            std::cout << "laserTransform.getOrigin().getZ():" << laserTransform.getOrigin().getZ() << std::endl;
        }
        loop_rate. sleep();
    }
}

代码注释： 

(1)代码的开始部分，先 include 4个头文件。在 u_listener.cpp 中使用一个新的类型叫作geometry_msgs/PointStamped,
在ROS 终端命令行使用 rosmsg show geometry_msgs/PointStamped 命令可以查看该类型，所以包含<geometry_msgs/PointStamped.h>头文件。

(2)ROS节点的主体函数是 int main(int argc，char** argv)，其参数定义和其他 C++语言程序一样。

(3)main()函数里，首先调用ros::init(argc，argv, "t_listener" )，进行该节点的初始化操作，函数的第三个参数是节点名称。

(4)接下来利用 Rate loop_rate(100) 控制节点运行的频率，与后面的 loop_rate.sleep()配合使用。

(5)创建一个监听器 listener，监听所有 TF 变换。

(6)在 base_laser 上声明一个点 laser_pos，用来转换得到 base_link上的点坐标。
首先创建一个 PointStamped类型的点 laser_pos，将这个点绑定到 base_laser 坐标系下，定义点的坐标为(0.3,0,0)。

(7)创建一个 PointStamped 类型的点 base_pos，用于存储转换到 base_link 上的点坐标

(8)为了连续不断地发送速度，使用一个 while(n.ok())循环，以 n.ok()返回值作为循环结束条件，可以让循环在程序关闭时正常退出。

(9)为了进行坐标变换，需要首先利用 waitForTransform()函数监听两个坐标之间的变换关系，
其中的第三个参数ros::Time(0)表示使用缓冲区中最新的 TF 数据。

(10)利用tansfommPoint()函数将 base_laser 坐标系中的 laser_pos 点变换到 base_link 坐标系中的 base_pos 点。

(11)输出坐标变换前 laser_pos 点的坐标值和坐标变换后 base_pos 点的坐标值。

(12)利用 lookupTransform() 函数获取最新的坐标变换关系，并输出 base_laser 坐标系原点在 base_link 坐标系中的坐标值。

(13)调用 loop_rate.sleep()兩数，按之前 loop_rate(100)设置的 100Hz将程序挂起。

代码编写完毕，需要将文件名添加到编译文件里才能进行编译。编译文件在tf_test-pkg的目录下，文件名为CMakeLisis.txt，
在CMakeLists.txt文件末尾，为 tf_listener.cpp 添加新的编译规则。内容如下:

add_executable( tf_listener src/tf_listener.cpp)
add_dependencies( tf_listener ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} )
target_link_libraries( tf_listener ${catkin_LIBRARIES} )

启动一个终端程序，执行如下指令编译工程。

cd ~/catkin_ws/
catkin_make

在新终端程序中输人以下指令，启动节点管理器

roscore

新建终端窗口，在终端程序中输人以下指令，运行 tf_test_pkg 功能包中的 tf_broadcaster 节点。

rosrun tf_test_pkg tf_broadcaster

新建终端窗口，在终端程序中输人以下指令:

rosrun tf_test_pkg tf_listener

可以看到在 base_laser 坐标系中的位置为(0.3,0,0)的点，在经过坐标变换后，在 base_link 坐标系中的位置为(0.4，0，0.2)，
而 base.laser 坐标系原点在 base_link 坐标系中的位置为( 0.1, 0，0.2 )。

可以通过指令査看 ROS 的节点网络情况。

rqt_graph

可以看到，编写的 tf_broadcaster 节点通过主题 "/tf" 向 tf_listener 节点发送消息包。
tf_listener 节点获得消息后，将坐标变换前后的结果发送到终端界面显示。

可以通过指令査看 TF 树状结构图。

rosrun rqt_tf_tree rqt_tf_tree

可以通过指令输出两个坐标系的坐标变换。

rosrun tf tf_echo /base_link base_laser