1、动态坐标变换
所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。
需求描述:
启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。
实现分析:
乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
将 pose 信息转换成 坐标系相对信息并发布
实现流程:C++ 与 Python 实现流程一致
新建功能包，添加依赖
创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)
创建坐标相对关系订阅方
执行
1.1、C++实现
1.创建功能包
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
配置
发布方
#include "ros/ros.h" // 引入 ROS 的基础库
#include "turtlesim/Pose.h" // 引入乌龟位姿消息类型
#include "tf2_ros/transform_broadcaster.h" // 引入 tf2 广播器库，用于发布坐标变换
#include "geometry_msgs/TransformStamped.h" // 引入转换消息类型，用于发布位姿信息
#include "tf2/LinearMath/Quaternion.h" // 引入四元数库，用于处理旋转
/*
发布方：需要订阅乌龟的位姿信息，转换成相对于窗体的坐标关系，并发布
准 备：
话题：/turtle1/pose
消息：/turtlesim/Pose
流程：
1.包含头文件
2.设置编码、初始化、NodeHandle
3.创建订阅对象，订阅 /turtle1/pose
4.回调函数处理订阅的消息：将位姿信息转换成坐标相对关系并发布（关注）
5.spin()
*/
void doPose(const turtlesim::Pose::ConstPtr& pose)
{
// 获取位姿信息，转换成坐标相对关系（核心），并发布
// a. 创建发布对象
static tf2_ros::TransformBroadcaster pub; // 创建一个广播器，用于发布坐标变换信息
// b. 组织被发布的数据
geometry_msgs::TransformStamped ts; // 创建一个TransformStamped消息，用于存储转换信息
ts.header.frame_id = "world"; // 设置参考坐标系为“world”
ts.header.stamp = ros::Time::now(); // 设置当前时间戳
ts.child_frame_id = "turtle1"; // 设置子坐标系为“turtle1”
// 坐标系偏移量设置
ts.transform.translation.x = pose->x; // 设置乌龟的 x 坐标
ts.transform.translation.y = pose->y; // 设置乌龟的 y 坐标
ts.transform.translation.z = 0; // 因为是二维问题，z 坐标设置为 0
// 坐标系四元数
/*
位姿信息中没有四元数，但是有个偏航角度，又已知乌龟是 2D，没有翻滚与俯仰角度，
所以可以得出乌龟的欧拉角：0，0，theta
*/
tf2::Quaternion qtn; // 创建四元数对象
qtn.setRPY(0, 0, pose->theta); // 设定四元数的欧拉角，只有偏航角（theta）有值，其他为 0
ts.transform.rotation.x = qtn.getX(); // 设置四元数的 x 分量
ts.transform.rotation.y = qtn.getY(); // 设置四元数的 y 分量
ts.transform.rotation.z = qtn.getZ(); // 设置四元数的 z 分量
ts.transform.rotation.w = qtn.getW(); // 设置四元数的 w 分量
// c. 发布转换信息
pub.sendTransform(ts); // 发布转换信息
}
int main(int argc, char *argv[])
{
// 2. 设置编码、初始化、NodeHandle
setlocale(LC_ALL,""); // 设置本地化（可以处理中文输出等）
ros::init(argc, argv, "dynamic_pub"); // 初始化 ROS 节点，节点名为 dynamic_pub
ros::NodeHandle nh; // 创建节点句柄，用于与 ROS 系统交互
// 3. 创建订阅对象，订阅 /turtle1/pose
ros::Subscriber sub = nh.subscribe("/turtle1/pose", 100, doPose); // 订阅“/turtle1/pose”话题，最多缓存 100 条消息，回调函数为 doPose
// 4. 回调函数处理订阅的消息：将位姿信息转换成坐标相对关系并发布（关注）
// 5. spin()
ros::spin(); // 启动 ROS 事件处理循环，回调函数会在这里被执行
return 0; // 返回 0，表示程序正常结束
}
发布方动态位姿信息变化
订阅方
#include "ros/ros.h" // 引入 ROS 的基础库
#include "tf2_ros/transform_listener.h" // 引入 TF2 监听器库，用于监听坐标变换
#include "tf2_ros/buffer.h" // 引入 TF2 缓存库，用于存储坐标变换信息
#include "geometry_msgs/PointStamped.h" // 引入点数据消息类型，包含坐标信息
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" // 引入 TF2 转换函数，用于坐标系之间转换
/*
订阅方：订阅发布的坐标相对关系，传入tf实现转换
流程：
1.包含头文件
2.编码 初始化 NodeHandle（必须的）
3.创建订阅对象； ----> 订阅坐标系相对关系
4.组织一个坐标点数据；
5.转换算法，需要调用TF内置实现
6.最后输出
*/
int main(int argc, char *argv[])
{
// 2.编码 初始化 NodeHandle（必须的）
setlocale(LC_ALL,""); // 设置本地化，可以正确显示中文等字符
ros::init(argc,argv,"dynamic_sub"); // 初始化 ROS 节点，节点名为 dynamic_sub
ros::NodeHandle nh; // 创建节点句柄，用于与 ROS 系统交互
// 3.创建订阅对象； ----> 订阅坐标系相对关系
// 3-1. 创建一个 buffer 缓存
tf2_ros::Buffer buffer; // 创建 tf2 缓存对象，用于存储从其他节点接收到的坐标变换信息
// 3-2. 再创建监听对象（监听对象可以将订阅的数据存入 buffer）
tf2_ros::TransformListener listener(buffer); // 创建 tf2 监听器，监听坐标变换，并将其存入 buffer
// 4. 组织一个坐标点数据；
geometry_msgs::PointStamped ps; // 创建一个点数据消息对象
// 参考的坐标系
ps.header.frame_id = "turtle1"; // 设置参考坐标系为“turtle1”
// 时间戳
ps.header.stamp = ros::Time(0.0); // 设置时间戳为 0（表示当前时间）
ps.point.x = 2.0; // 设置点的 x 坐标
ps.point.y = 3.0; // 设置点的 y 坐标
ps.point.z = 5.0; // 设置点的 z 坐标
// 添加休眠
// ros::Duration(5).sleep(); // （注释掉的代码）可以选择在此处让节点休眠 5 秒，等待 tf2 数据的准备
// 5. 转换算法，需要调用 TF 内置实现
ros::Rate rate(1); // 设置循环频率为 1Hz
while (ros::ok())
{
// 核心代码 ---- 将 ps 转换成相对于 base_link 的坐标点
geometry_msgs::PointStamped ps_out; // 创建一个输出坐标点消息对象
/*
调用了 buffer 转换函数 transform
参数1：被转换的坐标点
参数2：目标坐标系
返回值：输出的坐标点
ps1：调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
ps2：运行时候存在的问题：抛出一个异常 base_link不存在
原因：订阅数据是一个耗时操作，可能在调用 transform 转换函数时，坐标系的相对关系还没有订阅到，因此出现异常
解决：方案1：在调用转换函数前，执行休眠
方案2：进行异常处理
*/
try
{
ps_out = buffer.transform(ps, "world"); // 将 ps 坐标点从 "turtle1" 坐标系转换到 "world" 坐标系
// 6. 最后输出
ROS_INFO("转换后的坐标值为：（%.2f, %.2f, %.2f）,参考的坐标系：%s",
ps_out.point.x, ps_out.point.y, ps_out.point.z, ps_out.header.frame_id.c_str()); // 输出转换后的坐标信息
}
catch(const std::exception& e) // 捕获异常，如果转换失败，输出异常信息
{
// std::cerr << e.what() << '\n'; // 输出异常信息到标准错误流
ROS_INFO("异常消息：%s", e.what()); // 输出异常信息到 ROS log
}
rate.sleep(); // 按照设定的频率（1Hz）休眠
ros::spinOnce(); // 处理一次回调函数（如果有）
}
return 0; // 返回 0，表示程序正常结束
}
发布方订阅方动态坐标变换
1.2、Python实现
配置
发布方
#! /usr/bin/env python
import rospy # 导入 ROS 的 Python 库，用于节点创建、消息发布/订阅等
import tf2_ros # 导入 tf2 库，用于发布和监听坐标变换
import tf # 导入 tf 库，用于处理坐标变换中的四元数和欧拉角等
from turtlesim.msg import Pose # 导入乌龟位姿消息类型（Pose），包含 x, y, theta
from geometry_msgs.msg import TransformStamped # 导入 TransformStamped 消息类型，用于发布坐标变换
'''
发布方：订阅乌龟的位姿信息，转换成坐标系的相对关系，再发布
准备：
话题：/turtle1/pose
类型：/turtlesim/Pose
流程：
1.导包
2.初始化ros节点
3.创建订阅对象
4.回调函数处理订阅到的消息（核心）
5.spin（）
'''
def doPose(pose):
# 1. 创建发布坐标系相对关系的对象
pub = tf2_ros.TransformBroadcaster() # 创建 tf2 的 TransformBroadcaster 对象，用于发布坐标变换
# 2. 将 pose 转换成坐标系相对关系消息
ts = TransformStamped() # 创建 TransformStamped 对象，用于存储坐标变换
ts.header.frame_id = "world" # 设置父坐标系为 "world"
ts.header.stamp = rospy.Time.now() # 设置当前时间戳
ts.child_frame_id = "turtle1" # 设置子坐标系为 "turtle1"
# 子级坐标系相对于父级坐标系的偏移量
ts.transform.translation.x = pose.x # 设置乌龟的 x 坐标
ts.transform.translation.y = pose.y # 设置乌龟的 y 坐标
ts.transform.translation.z = 0 # 乌龟是 2D 的，所以 z 坐标设置为 0
# 四元数
# 从欧拉角转换为四元数
'''
乌龟是 2D 的，不存在 X 上的翻滚，Y 上的俯仰，只有 Z 上的偏航角（theta）
0, 0, pose.theta
'''
qtn = tf.transformations.quaternion_from_euler(0, 0, pose.theta) # 将偏航角转换为四元数
ts.transform.rotation.x = qtn[0] # 设置四元数的 x 分量
ts.transform.rotation.y = qtn[1] # 设置四元数的 y 分量
ts.transform.rotation.z = qtn[2] # 设置四元数的 z 分量
ts.transform.rotation.w = qtn[3] # 设置四元数的 w 分量
# 3. 发布
pub.sendTransform(ts) # 发布变换信息
if __name__ == "__main__":
# 2. 初始化 ros 节点
rospy.init_node("dynamic_pub_p") # 初始化 ROS 节点，节点名为 "dynamic_pub_p"
# 3. 创建订阅对象
sub = rospy.Subscriber("/turtle1/pose", Pose, doPose, queue_size=100) # 订阅 "/turtle1/pose" 话题，回调函数为 doPose
# 4. 回调函数处理订阅到的消息（核心）
# 5. spin()
rospy.spin() # 保持节点运行并调用回调函数
订阅方
#! /usr/bin/env python
import rospy # 导入 ROS 的 Python 库，用于节点创建、消息发布/订阅等
import tf2_ros # 导入 tf2 库，用于坐标变换的监听和发布
# 不要使用 geometry_msgs, 需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import tf2_geometry_msgs # 导入 tf2_geometry_msgs 库，用于处理 tf2 坐标变换中的几何消息类型
# from geometry_msgs.msg import PointStamped # （注释掉的）原本如果不使用 tf2 内置的消息类型时可以使用该库
'''
订阅方：订阅坐标变换消息，传入被转换的坐标点，调用转换算法
流程：
1.导包
2.初始化
3.创建订阅对象
4.组织被转换的坐标点
5.转换逻辑实现，调用 tf 封装的算法
6.输出结果
'''
if __name__ == "__main__":
# 2. 初始化
rospy.init_node("dynamic_sub_p") # 初始化 ROS 节点，节点名为 "dynamic_sub_p"
# 3. 创建订阅对象
# 3-1. 创建缓存对象
buffer = tf2_ros.Buffer() # 创建 tf2 缓存对象，用于存储坐标变换信息
# 3-2. 创建订阅对象（将缓存传入）
sub = tf2_ros.TransformListener(buffer) # 创建 TransformListener 对象，监听 tf2 坐标变换，并将其存入缓存
# 4. 组织被转换的坐标点
ps = tf2_geometry_msgs.PointStamped() # 创建 PointStamped 消息对象，用于存储待转换的坐标点
# 时间戳 -- 0
ps.header.stamp = rospy.Time() # 设置时间戳为当前时间
# 坐标系
ps.header.frame_id = "turtle1" # 设置参考坐标系为 "turtle1"
ps.point.x = 2.0 # 设置坐标点的 x 坐标
ps.point.y = 3.0 # 设置坐标点的 y 坐标
ps.point.z = 5.0 # 设置坐标点的 z 坐标
# 5. 转换逻辑实现，调用 tf 封装的算法
rate = rospy.Rate(1) # 设置循环频率为 1 Hz
while not rospy.is_shutdown(): # 当 ROS 节点没有被关闭时继续循环
try:
# 转换实现
ps_out = buffer.transform(ps, "world") # 将坐标点 ps 从 "turtle1" 坐标系转换到 "world" 坐标系
# 6. 输出结果
rospy.loginfo("转换后的坐标：（%.2f, %.2f, %.2f）,参考的坐标系：%s",
ps_out.point.x, ps_out.point.y, ps_out.point.z, ps_out.header.frame_id) # 输出转换后的坐标点
except Exception as e:
rospy.loginfo("错误提示！！！:%s", e) # 如果发生异常，输出错误信息
rate.sleep() # 按照设定的频率休眠
2、多坐标变换
需求描述:
现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
实现分析:
首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换
最后，还要实现坐标点的转换
实现流程:C++ 与 Python 实现流程一致
新建功能包，添加依赖
创建坐标相对关系发布方(需要发布两个坐标相对关系)
创建坐标相对关系订阅方
执行
准备工作
C++实现
发布方
创建launch文件 发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
订阅方
配置
编写cpp文件
#include "ros/ros.h" // ROS基础功能头文件
#include "tf2_ros/transform_listener.h" // TF2变换监听器头文件
#include "tf2/LinearMath/Quaternion.h" // 四元数相关操作
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" // TF2与geometry_msgs转换
#include "geometry_msgs/TransformStamped.h" // 变换消息类型
#include "geometry_msgs/PointStamped.h" // 带时间戳的坐标点类型
/*
订阅方实现功能：
1.计算 son1 与 son2 的相对关系
2.计算 son1 中的某个坐标点在 son2 中的坐标值
流程：
1.包含头文件
2.编码 初始化 NodeHandle
3.创建订阅对象
4.编写解析逻辑
5.spinOnce()
*/
int main(int argc, char *argv[])
{
// 2.初始化ROS节点和NodeHandle
setlocale(LC_ALL,""); // 设置本地化，支持中文输出
ros::init(argc,argv,"tfs_sub"); // 初始化节点，节点名为"tfs_sub"
ros::NodeHandle nh; // 创建节点句柄
// 3.创建TF2缓冲区和监听器
tf2_ros::Buffer buffer; // 创建TF2缓冲区，用于存储变换数据
tf2_ros::TransformListener sub(buffer); // 创建TF2监听器，订阅/tf话题并将数据存入缓冲区
// 4.创建并初始化一个在son1坐标系中的点
geometry_msgs::PointStamped psAtSon1; // 创建一个带时间戳的坐标点
psAtSon1.header.stamp = ros::Time::now(); // 设置时间戳为当前时间
psAtSon1.header.frame_id = "son1"; // 设置坐标系为son1
psAtSon1.point.x = 1.0; // 设置x坐标
psAtSon1.point.y = 2.0; // 设置y坐标
psAtSon1.point.z = 3.0; // 设置z坐标
ros::Rate rate(1); // 设置循环频率为1Hz
while (ros::ok()) // 主循环
{
try
{
// 功能1：计算son1相对于son2的坐标变换
/*
lookupTransform函数参数说明：
参数1：目标坐标系 (son2)
参数2：源坐标系 (son1)
参数3：ros::Time(0) 表示获取最新的可用变换
返回值：geometry_msgs::TransformStamped 源坐标系相对于目标坐标系的变换
*/
geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("son2","son1",ros::Time(0));
ROS_INFO("son1 相对于 son2 的信息：父级：%s，子级：%s 偏移量（%.2f,%.2f,%.2f）",
son1ToSon2.header.frame_id.c_str(), // 父坐标系(son2)
son1ToSon2.child_frame_id.c_str(), // 子坐标系(son1)
son1ToSon2.transform.translation.x, // x方向偏移
son1ToSon2.transform.translation.y, // y方向偏移
son1ToSon2.transform.translation.z); // z方向偏移
// 功能2：将son1中的点转换到son2坐标系
/*
transform函数参数说明：
参数1：要转换的点(psAtSon1)
参数2：目标坐标系(son2)
返回值：在目标坐标系中的点坐标
*/
geometry_msgs::PointStamped psAtSon2 = buffer.transform(psAtSon1,"son2");
ROS_INFO("坐标点在 Son2 中的值(%.2f,%.2f,%.2f)",
psAtSon2.point.x, // 转换后的x坐标
psAtSon2.point.y, // 转换后的y坐标
psAtSon2.point.z); // 转换后的z坐标
}
catch(const std::exception& e) // 捕获异常
{
ROS_INFO("错误提示：%s ！！！！！",e.what()); // 打印错误信息
}
rate.sleep(); // 控制循环频率
ros::spinOnce(); // 处理回调函数
}
return 0;
}编译 启动roscore 执行
Python实现
配置 添加可执行权限
编写py文件
#!/usr/bin/env python
"""
需求:
现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，
son1 相对于 world，以及 son2 相对于 world 的关系是已知的，
求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
实现流程:
1.导包
2.初始化 ROS 节点
3.创建 TF 订阅对象
4.调用 API 求出 son1 相对于 son2 的坐标关系
5.创建一依赖于 son1 的坐标点，调用 API 求出该点在 son2 中的坐标
6.spin
"""
# 1.导包
import rospy # ROS Python接口
import tf2_ros # TF2 Python接口
from geometry_msgs.msg import TransformStamped # 变换消息类型
from tf2_geometry_msgs import PointStamped # 带时间戳的坐标点类型
if __name__ == "__main__":
# 2.初始化 ROS 节点
rospy.init_node("frames_sub_p") # 创建节点，节点名为"frames_sub_p"
# 3.创建 TF 订阅对象
buffer = tf2_ros.Buffer() # 创建TF2缓冲区，存储变换数据
listener = tf2_ros.TransformListener(buffer) # 创建监听器，订阅/tf话题
rate = rospy.Rate(1) # 设置循环频率为1Hz
while not rospy.is_shutdown(): # 主循环
try:
# 4.调用API获取son1相对于son2的坐标关系
# lookup_transform参数说明：
# target_frame: 目标坐标系(son2)
# source_frame: 源坐标系(son1)
# time: rospy.Time(0)表示获取最新可用变换
tfs = buffer.lookup_transform("son2","son1",rospy.Time(0))
rospy.loginfo("son1 与 son2 相对关系:")
rospy.loginfo("父级坐标系:%s",tfs.header.frame_id) # 父坐标系(son2)
rospy.loginfo("子级坐标系:%s",tfs.child_frame_id) # 子坐标系(son1)
rospy.loginfo("相对坐标:x=%.2f, y=%.2f, z=%.2f",
tfs.transform.translation.x, # x方向偏移
tfs.transform.translation.y, # y方向偏移
tfs.transform.translation.z, # z方向偏移
)
# 5.创建son1中的点并转换到son2坐标系
point_source = PointStamped() # 创建源坐标点
point_source.header.frame_id = "son1" # 设置坐标系为son1
point_source.header.stamp = rospy.Time.now() # 设置当前时间戳
point_source.point.x = 1 # 设置x坐标
point_source.point.y = 1 # 设置y坐标
point_source.point.z = 1 # 设置z坐标
# transform参数说明：
# point_source: 要转换的点
# target_frame: 目标坐标系(son2)
# timeout: 超时时间(0.5秒)
point_target = buffer.transform(point_source,"son2",rospy.Duration(0.5))
rospy.loginfo("point_target 所属的坐标系:%s",point_target.header.frame_id) # 转换后的坐标系
rospy.loginfo("坐标点相对于 son2 的坐标:(%.2f,%.2f,%.2f)",
point_target.point.x, # 转换后的x坐标
point_target.point.y, # 转换后的y坐标
point_target.point.z # 转换后的z坐标
)
except Exception as e: # 捕获异常
rospy.logerr("错误提示:%s",e) # 打印错误信息
rate.sleep() # 控制循环频率
# 6.spin
# rospy.spin()
3、坐标系关系查看
在机器人系统中，涉及的坐标系有多个，为了方便查看，ros 提供了专门的工具，可以用于生成显示坐标系关系的 pdf 文件，该文件包含树形结构的坐标系图谱。
准备
首先调用rospack find tf2_tools查看是否包含该功能包，如果没有，请使用如下命令安装:
sudo apt install ros-noetic-tf2-tools
使用
启动坐标系广播程序之后，运行如下命令:
rosrun tf2_tools view_frames.py
可以直接进入目录打开文件，或者调用命令查看文件:evince frames.pdf
