目录
多坐标变换与静态/动态坐标变换的不同之处
tf坐标转换的实质
你也许会被静态/动态/多坐标转换弄晕了头，但是你只要认清坐标转换的实质就会恍然大悟。无论是哪种坐标转换，其实质就是利用tf2_ros::Buffer类对象中缓存的坐标间的相互关系将坐标点基于X坐标系的坐标转化至基于Y坐标系的坐标。
多坐标变换
我们以坐标系A、坐标系B、坐标系C、坐标系D三个坐标系相互转化为例进行说明：
准确的来说lookupTransfrom函数的作用是取得树状图中两个从未有联系的坐标系得相对关系：坐标系C与坐标系D的相对关系。可以取得树状图中已经建立联系的坐标系间的相对关系吗？可以，但是没有什么必要，如果我们仅仅使用已知的坐标系关系那么lookupTransfrom函数有无均可。
静态/动态坐标变换
我们会发现：两者都有的地方就是“调用transfrom函数进行坐标转换”。此外，两者不同之处在于坐标系间的转换关系获取，多坐标系转换中需要调用lookupTransfrom函数对buffer缓冲区内的数据进行复杂计算得到任意两个坐标系的转换关系，而静态/动态坐标系转换关系只需订阅static_tf话题即可得到。
坐标变换详解
旋转矩阵：
坐标变换最通俗易懂的解释（推到+图解）_超级霸霸强的博客-CSDN博客
https://blog.csdn.net/weixin_45590473/article/details/122848202
什么是TF2？
tf2是一个用来描述物体空间位置的工具。我们知道，用一个三维向量和一个四元数就可以描述一个物体相对于一个参考系的位置与姿态，tf2的工作属于“小而美”，它只专注于在ROS2中提供这样一个功能，就是描述一组物体之间的相对位置关系。
为何要使用TF2？
TF2可以用于描述机器人各关节的相对位置，大大方便了运动学求解。不仅如此，在多机器人的场景中，tf2还可以用于描述机器人之间的相对位置，从而执行跟随、避障等操作。此外，tf2也可以用于描述目标物体与机器人的相对位置。总而言之，我们只需提供坐标系的相对关系（旋转关系+位置偏移关系）即可，无需关注中间求解过程。
各个功能包的作用
我们知道使用tf坐标转换除了需要roscpp、rospy、std_msgs这三个基本的功能包外，还需要tf2、tf2_geometry_msgs、geometry_msgs、tf2_ros这四个额外的功能包，我们需要知道他们分别的作用，这样有助于我们记忆：
1. tf2功能包：该功能包用于根据坐标系信息以及坐标系间的关系计算得到坐标系之间坐标变换关系（向量/坐标系的旋转）；
2. geometry_msgs功能包：里面包含了常见坐标系的数据类型（eg. TransformStamped数据类型包含了参考坐标系的名称，参考坐标系创建的时间以及子坐标系相较于参考坐标系的相对位置（旋转+平移）；PointStamped数据类型包含了子坐标系中点的坐标）；
3. tf2_ros功能包：该功能包用于在节点之间发送信息并进行相关操作，这些都是基于内部的NodeHandle对象实现的（eg. Tf2_ros功能包中StaticTransformBroadcaster 用于发布static_tf话题消息；TransformListener用于订阅static_tf话题消息；Buffer用于缓存订阅节点订阅到的static_tf话题消息）；
4. tf2_geometry_msgs功能包：该功能包的主要作用就是根据“接收到的坐标系相互关系的信息（TransformStamped数据）以及自身子坐标系中坐标点的信息（PointStamped数据）”通过tf2功能包计算得到“参考坐标系下坐标点的位置信息”。此外类似的还有如下的功能包：
这些功能包的功能就是充当数据类型转换的媒介，将其他数据类型转换为tf2坐标变换工具可以识别的数据类型。
注意：tf2_geometry_msgs功能包可以根据tf2功能包得到的坐标变换关系结合被转换坐标系坐标点坐标得出目标坐标系坐标点的坐标，这要与tf2功能包得出的坐标变换关系区分开！
整体功能的实现逻辑
我们要实现坐标的变换就必须清楚订阅者和发布者所实现的功能，即整体功能的实现逻辑：
我们读完下面的发布者/订阅者的代码实现可能会有以下几点疑惑：
1. 为什么.msg消息的发布如下所示的代码而不调用节点句柄呢？
// 创建发布者对象的头文件
#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); 因为ROS系统为了可以让我们向傻瓜操作一样进行坐标变换，将一些用于声明坐标系信息及相对关系等的接口暴露给我们并且将内部的逻辑实现进行了封装。就比如：无论是订阅者对象TransformListener还是发布者对象StaticTransformBroadcaster都内部封装了NodeHandle用于消息文件的发布与订阅，并且两者订阅的话题也已经系统内定我们无需关注。
2. 为什么声明订阅者对象时需要额外传给listener对象一个buffer对象？
我们仔细的关注代码会注意到：订阅者对象中除了析构函数什么都没有，但是buffer对象中却拥有很多成员函数，包括转换关系的获取函数等。再结合：
1. 订阅者接收到的是用于坐标系转换的四元数；
2. 订阅者端源代码中包含子坐标系中坐标点X/Y/Z轴坐标的声明；
我们可以得到如下结论：
订阅者的作用仅仅是订阅static_tf话题从而获得坐标转换关系，并将其存入buffer等待调用。我们可以使用buffer内置的函数调用我们获得的坐标转换关系将子坐标系下的坐标点坐标转化至参考坐标系下的坐标点坐标。
3. 调用buffer中transform函数时，不添加tf2_geometry_msgs.h头文件就报如下错误提示？
我们从逻辑上来理解一下transform函数，该函数起到如下作用：
而tf2_geometry_msgs.h可以结合tf2的坐标转换关系将子坐标系坐标点坐标转换为参考坐标系坐标，即tf2_geometry_msgs.h定义了参考坐标系坐标点坐标的计算法则。
发布者的实现
Tf2坐标转换中发布消息是.msg类型的，因此我们无论是编写发布方还是订阅方都须遵循话题通信的实现逻辑。
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::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(); 具体参数及含义如下所示：
其中，我们用到了坐标变换工具包tf2：
// 相关头文件
#include "tf2/LinearMath/Quaternion.h"
// 欧拉角转四元数
tf2::Quaternion qtn;
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();
} 发布信息按照话题通信的格式编写即可。
订阅者的实现
1. 我们前面介绍过我们需要先创建订阅者对象，然后基于订阅者对象一个缓冲区用于存放接收到的坐标转换关系：
// 创建订阅者
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer); 订阅者的成功创建意味着“节点句柄创建的完成+消息订阅的成功”。
2. 然后，我们需要轮询接收消息，这一点与一般的话题通信有很大的不同，一般的话题通信大都采用ros::spin()回旋函数来调用回调函数进行数据的处理。但是我们要意识到tf坐标转换的特殊性，我们要的不是坐标转换关系，因此设置回调函数毫无意义，我们要的是使用坐标转换关系结合子坐标系的坐标点坐标去求解参考坐标系的坐标点坐标。代码如下所示：
#include "ros/ros.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
int main(int argc,char* argv[])
{
setlocale(LC_ALL,"");
// initial the node
ros::init(argc,argv,"static_sub");
// 创建订阅者
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
// while轮询进行坐标点坐标的转换
ros::Rate rate(1); // 轮询频率的设置
while(ros::ok()){
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "laser";
point_laser.header.stamp = ros::Time::now();
point_laser.point.x = 0.1;
point_laser.point.y = 0.1;
point_laser.point.z = 0.1;
try
{
geometry_msgs::PointStamped point_base_link = buffer.transform<geometry_msgs::PointStamped>(point_laser,"base_link");
ROS_INFO("frame based on base_link:%f,%f,%f\n\t",
point_base_link.point.x,point_base_link.point.y,point_base_link.point.z);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
ROS_INFO("----waiting-----\n\t");
}
rate.sleep();
}
return 0;
} 其中，最重要的有以下几点：
1. 设置坐标点坐标的信息：
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "laser";
point_laser.header.stamp = ros::Time::now();
point_laser.point.x = 0.1;
point_laser.point.y = 0.1;
point_laser.point.z = 0.1; geometry_msgs::PointStamped类型中包含着该坐标系的名称，该坐标系创建的时间，该坐标系中坐标点的坐标。
2. 参考坐标系中坐标点坐标的计算
geometry_msgs::PointStamped point_base_link = buffer.transform<geometry_msgs::PointStamped>(point_laser,"base_link"); 3. 使用try-catch异常捕捉语法
如果我们不使用try-catch块，代码如下所示：
// 轮询接收
ros::Rate rate(1);
while(ros::ok()){
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "laser";
point_laser.header.stamp = ros::Time::now();
point_laser.point.x = 0.1;
point_laser.point.y = 0.1;
point_laser.point.z = 0.1;
geometry_msgs::PointStamped point_base_link = buffer.transform<geometry_msgs::PointStamped>(point_laser,"base_link");
ROS_INFO("frame based on base_link:%f,%f,%f\n\t",
point_base_link.point.x,point_base_link.point.y,point_base_link.point.z);
} 运行结果：
我们会发现：报错了！这是为什么呢？
仔细地读者会发现：错误为“名为base_link坐标系不存在”。首先，为了找出错误的根源，我们要知道“ROS如何判断出base_link坐标系不存在的”：
// 发布者中base_link坐标系创建时间戳
geometry_msgs::TransformStamped data;
data.header.frame_id = "base_link";
data.header.stamp = ros::Time::now();
data.child_frame_id = "laser";
......
// 订阅者中laser坐标系创建时间戳
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "laser";
point_laser.header.stamp = ros::Time::now();
point_laser.point.x = 0.1;
point_laser.point.y = 0.1;
point_laser.point.z = 0.1; 在各个坐标系创建时，会有时间戳记录下该坐标系创建的具体时间，坐标系转化必须具有实时性，也就是说：time=0s时创建的坐标系之间才可以相互转化。time=1s时的坐标系与time=0s时的坐标系是不可以相互转化的，因为这两个坐标系创立的时间根本不同。ROS系统正是通过比较两个坐标系创建的时间来确定到底这两个坐标系是否可以转化，如果创建的时间差距较大(秒级别的时间延迟就算比较大的时间差了)系统就会报出如上的错误提示。
如果我们使用try-catch块则：
Try-catch的意义就在于捕获异常但是允许异常的存在，继续轮询等待base_link坐标系的出现。如果我们不使用try-catch，那么系统一旦碰见异常就会立刻终止节点的运行。
添加try-catch块的意义
我们无论通过launch文件还是命令行窗口启动坐标系信息的发布/订阅节点，发布/订阅节点不可能同时启动，但是坐标系转换的实时性不允许“在同一时刻发布/订阅节点仅有一个被启动”。那我们就使用try-catch捕获异常，然后允许异常的存在，我们只在catch程序块中打印出异常并不进行终止程序的处理操作。不久之后，当订阅/发布端已经均处于运行状态，那么异常自然而然地会消失。
为何不使用一个CPP源文件去完成坐标变换？
https://blog.csdn.net/weixin_45590473/article/details/123015364项目的另一种实现形式：命令行工具+订阅端
相较于上一种静态坐标变换的实现形式，我们使用tf2内置的命令行工具创建坐标相对关系的发布方，然后我们在订阅者的缓存区当中调用坐标系转换关系计算函数计算出任意两个坐标系的转换关系，最后，我们发送坐标转换请求进行坐标系的转换。
发布者的实现
我们可以用tf2内置的publisher来进行坐标系消息的发布，tf和tf_ros功能包中均有这样的命令行工具：
1. tf功能包中static_transform_publisher的使用：
Args中有两类参数：
9个参数分别为：子坐标系X/Y/Z轴相对于参考坐标系在X/Y/Z轴偏移的距离以及相对于X/Y/Z轴旋转的角度、参考坐标系名称（目标坐标系）、子坐标系（原坐标系）名称、坐标系信息发布间隔时间（单位为ms）；
10个参数分别为：子坐标系X/Y/Z轴相对于参考坐标系在X/Y/Z轴偏移的距离以及相对于X/Y/Z轴旋转的四元数、参考坐标系名称（目标坐标系）、子坐标系（原坐标系）名称、坐标系信息发布间隔时间（单位为ms）；
2. tf2_ros功能包中static_transform_publisher的使用：
Args中分为两类：
8个参数分别为：子坐标系X/Y/Z轴相对于参考坐标系在X/Y/Z轴偏移的距离以及相对于X/Y/Z轴旋转的角度、参考坐标系名称（目标坐标系）、子坐标系（原坐标系）名称；
9个参数分别为：子坐标系X/Y/Z轴相对于参考坐标系在X/Y/Z轴偏移的距离以及相对于X/Y/Z轴旋转的四元数、参考坐标系名称（目标坐标系）、子坐标系（原坐标系）名称。
我们使用tf2_ros功能包中的publisher发布端实现方式：（基于launch文件）
<launch>
<node pkg="tf2_ros" name="son1" type="static_transform_publisher" args="1 2 3 10 30 20 /world /son1" output="screen"/>
<node pkg="tf2_ros" name="son2" type="static_transform_publisher" args="-1 -5 2 40 10 50 /world /son2" output="screen"/>
</launch> 订阅者的实现
1. 首先，我们需要实例化ros系统中坐标系相对关系的监听者以及监听者的缓冲区：
ros::init(argc,argv,"another_static_sub");
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer); 2. 需要使用lookupTransform函数得到缓冲区中任意两个坐标系间的坐标转换关系，并且进行坐标转换：
ros::Rate rate(1);
while(ros::ok()){
try
{
geometry_msgs::TransformStamped Son12Son2 = buffer.lookupTransform("son2","son1",ros::Time(0));
geometry_msgs::PointStamped PointInSon1;
PointInSon1.header.frame_id = "son1";
PointInSon1.header.stamp = ros::Time();
PointInSon1.point.x = 1;
PointInSon1.point.y = 2;
PointInSon1.point.z = 3;
geometry_msgs::PointStamped PointInSon2 = buffer.transform(PointInSon1,"son2");
ROS_INFO("PointInSon2:%f,%f,%f",PointInSon2.point.x,PointInSon2.point.y,PointInSon2.point.z);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
rate.sleep();
} 注意：
geometry_msgs::TransformStamped Son12Son2 = buffer.lookupTransform("son2","son1",ros::Time(0));该句程序中的Son12Son2变量虽然没被使用到，但是该句必须有。因为lookupTransfrom函数的作用不仅是输出一个表示坐标系转换关系的数据类型，同时也将表征着坐标系转换关系的数据类型存入buffer中。也就是说没有这一句，那么buffer就没有坐标系son1到son2的转换关系，进而transfrom函数也就无法调用坐标系转换关系完成坐标点坐标的转换！
总程序实现如下所示：
#include "ros/ros.h"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/PointStamped.h"
int main(int argc,char* argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"another_static_sub");
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate rate(1);
while(ros::ok()){
try
{
geometry_msgs::TransformStamped Son12Son2 = buffer.lookupTransform("son2","son1",ros::Time(0));
geometry_msgs::PointStamped PointInSon1;
PointInSon1.header.frame_id = "son1";
PointInSon1.header.stamp = ros::Time();
PointInSon1.point.x = 1;
PointInSon1.point.y = 2;
PointInSon1.point.z = 3;
geometry_msgs::PointStamped PointInSon2 = buffer.transform(PointInSon1,"son2");
ROS_INFO("PointInSon2:%f,%f,%f",PointInSon2.point.x,PointInSon2.point.y,PointInSon2.point.z);
}
catch(const std::exception& e)
{
std::cerr << e.what() << '\n';
}
rate.sleep();
}
return 0;
} 
687
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
