Ros2 TF工具

1. 简介：

tf2是一个随时间跟踪多个坐标系的功能包，使用tf2功能包总的来说就只有两步：1、监听TF变换2、广播TF变换你也许会被静态/动态/多坐标转换弄晕了头，但是你只要认清坐标转换的实质就会恍然大悟。无论是哪种坐标转换，其实质就是利用tf2_ros::Buffer类对象中缓存的坐标间的相互关系将坐标点基于X坐标系的坐标转化至基于Y坐标系的坐标。

1.1 多坐标变换
我们以坐标系A、坐标系B、坐标系C、坐标系D三个坐标系相互转化为例进行说明：

准确的来说lookupTransfrom函数的作用是取得树状图中两个从未有联系的坐标系得相对关系：坐标系C与坐标系D的相对关系。可以取得树状图中已经建立联系的坐标系间的相对关系吗？可以，但是没有什么必要，如果我们仅仅使用已知的坐标系关系那么lookupTransfrom函数有无均可。

1.2 静态/动态坐标变换

我们会发现：两者都有的地方就是“调用transfrom函数进行坐标转换”。此外，两者不同之处在于坐标系间的转换关系获取，多坐标系转换中需要调用lookupTransfrom函数对buffer缓冲区内的数据进行复杂计算得到任意两个坐标系的转换关系，而静态/动态坐标系转换关系只需订阅static_tf话题即可得到。

1.3 各功能包作用
我们知道使用tf坐标转换除了需要roscpp、rospy、std_msgs这三个基本的功能包外，还需要tf2、tf2_geometry_msgs、geometry_msgs、tf2_ros这四个额外的功能包，我们需要知道他们分别的作用，这样有助于我们记忆：

• tf2功能包：该功能包用于根据坐标系信息以及坐标系间的关系计算得到坐标系之间坐标变换关系（向量/坐标系的旋转）

• geometry_msgs功能包：里面包含了常见坐标系的数据类型（eg. TransformStamped数据类型包含了参考坐标系的名称，参考坐标系创建的时间以及子坐标系相较于参考坐标系的相对位置（旋转+平移）；PointStamped数据类型包含了子坐标系中点的坐标）

• tf2_ros功能包：该功能包用于在节点之间发送信息并进行相关操作，这些都是基于内部的NodeHandle对象实现的（eg. Tf2_ros功能包中StaticTransformBroadcaster 用于发布static_tf话题消息；TransformListener用于订阅static_tf话题消息；Buffer用于缓存订阅节点订阅到的static_tf话题消息）

• tf2_geometry_msgs功能包：该功能包的主要作用就是根据“接收到的坐标系相互关系的信息（TransformStamped数据）以及自身子坐标系中坐标点的信息（PointStamped数据）”通过tf2功能包计算得到“参考坐标系下坐标点的位置信息”。此外类似的还有如下的功能包：
  
  这些功能包的功能就是充当数据类型转换的媒介，将其他数据类型转换为tf2坐标变换工具可以识别的数据类型。
  注意: tf2_geometry_msgs功能包可以根据tf2功能包得到的坐标变换关系结合被转换坐标系坐标点坐标得出目标坐标系坐标点的坐标，这要与tf2功能包得出的坐标变换关系区分开！

2.坐标变换详解

四元数：
彻底搞懂“旋转矩阵/欧拉角/四元数”，让你体会三维旋转之美
旋转矩阵：
坐标变换最通俗易懂的解释（推到+图解）

3.整体功能的实现逻辑

我们要实现坐标的变换就必须清楚订阅者和发布者所实现的功能，即整体功能的实现逻辑：

我们读完下面的发布者/订阅者的代码实现可能会有以下几点疑惑：

// 创建发布者对象的头文件  
#include "tf2_ros/static_transform_broadcaster.h"  
// 创建发布者对象  
tf2_ros::StaticTransformBroadcaster pub;  
// 创建订阅者对象的头文件  
#include "tf2_ros/transform_listener.h"  
#include "tf2_ros/buffer.h"  
// 创建订阅者对象  
tf2_ros::Buffer buffer;  
tf2_ros::TransformListener listener(buffer);

1. 为什么.msg消息的发布如下所示的代码而不调用节点句柄(传个空nullptr也行)呢？
   因为ROS系统将一些用于声明坐标系信息及相对关系等的接口暴露给我们并且将内部的逻辑实现进行了封装。就比如：无论是订阅者对象TransformListener还是发布者对象StaticTransformBroadcaster都内部封装了NodeHandle用于消息文件的发布与订阅，并且两者订阅的话题也已经系统内定我们无需关注。
2. 为什么声明订阅者对象时需要额外传给listener对象一个buffer对象？
   我们仔细的关注代码会注意到：订阅者对象中除了析构函数什么都没有，但是buffer对象中却拥有很多成员函数，包括转换关系的获取函数等。再结合：订阅者接收到的是用于坐标系转换的四元数；订阅者端源代码中包含子坐标系中坐标点X/Y/Z轴坐标的声明；可以得到如下结论： 订阅者的作用仅仅是订阅static_tf话题从而获得坐标转换关系，并将其存入buffer等待调用。我们可以使用buffer内置的函数调用我们获得的坐标转换关系将子坐标系下的坐标点坐标转化至参考坐标系下的坐标点坐标。
   
3. 调用buffer中transform函数时，不添加tf2_geometry_msgs.h头文件就报如下错误提示？
   我们从逻辑上来理解一下transform函数，该函数起到如下作用：tf2_geometry_msgs.h可以结合tf2的坐标转换关系将子坐标系坐标点坐标转换为参考坐标系坐标，即tf2_geometry_msgs.h定义了参考坐标系坐标点坐标的计算法则。

4. TF广播-发布者

Tf2坐标转换中发布消息是.msg类型的，因此我们无论是编写发布方还是订阅方都须遵循话题通信的实现逻辑。
4.1 发布者实现
（1）首先，我们创建发布对象：

// 头文件  
#include "tf2_ros/static_transform_broadcaster.h"  
// create publisher  
tf2_ros::StaticTransformBroadcaster pub;

（2）在发布者中最重要的就是组织发布的信息，代码实现如下：

// 所需头文件  
#include "geometry_msgs/TransformStamped.h"  
// 组织信息  
// organize the data  
geometry_msgs::TransformStamped data;  
data.header.frame_id = "base_link";//参考坐标系
data.header.stamp = ros::Time::now();  
data.child_frame_id = "laser";// 子坐标系
// 子坐标系相对于参考坐标系在参考坐标系X/Y/Z轴方向平移的距离
data.transform.translation.x = 0.1;  
data.transform.translation.y = 0.5;  
data.transform.translation.z = 0.3;  
// 子坐标系各轴相对于参考坐标系各坐标轴旋转的角度 
// 这里用到了坐标变换工具包tf2 需要包含头文件，可放在顶部
#include "tf2/LinearMath/Quaternion.h" 
tf2::Quaternion qtn; // 调用tf2坐标变换工具
//欧拉角转四元数
qtn.setRPY(30,10,5);  
data.transform.rotation.w = qtn.getW();  
data.transform.rotation.x = qtn.getX();  
data.transform.rotation.y = qtn.getY();  
data.transform.rotation.z = qtn.getZ();  

具体参数及含义如下所示：

注意：我们要做的是将子坐标系到参考坐标系的变换关系传输给订阅static_tf话题的订阅者。

（3）信息的发布

// 不断轮询进行坐标系信息的发布  
ros::Rate rate(1);  
while(ros::ok()){
     
    pub.sendTransform(data);  
    rate.sleep();  
}  

4.2 广播器示例：turtle_tf_broadcaster.cpp

#include "turtlesim/msg/pose.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

std::string turtle_name;
//节点句柄
rclcpp::Node::SharedPtr node_handle = nullptr;

void pose_callback(const turtlesim::msg::Pose::SharedPtr pose)
{
   
    //tf广播器
    static tf2_ros::TransformBroadcaster pose_broadcaster_(node_handle);
    //广播器消息类型实例化
    geometry_msgs::msg::TransformStamped pose_tf;
    //根据海龟当前位姿，发布对世界坐标系的变换
    pose_tf.header.stamp = node_handle->now();
    pose_tf.header.frame_id = "world"