第 5 章 ROS 常用组件 1 —— TF 坐标变换_静态坐标变换 tf01_static / 动态坐标变换 tf02_dynamic

00 学习目标

在ROS中内置一些比较实用的工具，通过这些工具可以方便快捷的实现某个功能或调试程序，从而提高开发效率，本章主要介绍ROS中内置的如下组件：

• TF坐标变换，实现不同类型的坐标系之间的转换；
• rosbag 用于录制ROS节点的执行过程并可以重放该过程；
• rqt 工具箱，集成了多款图形化的调试工具。

本章预期达成的学习目标：

• 了解 TF 坐标变换的概念以及应用场景
• 能够独立完成TF案例:小乌龟跟随
• 可以使用 rosbag 命令或编码的形式实现录制与回放
• 能够熟练使用rqt中的图形化工具

01 TF 坐标变换

机器人系统上，有多个传感器，如激光雷达、摄像头等，有的传感器是可以感知机器人周边的物体方位(或者称之为:坐标，横向、纵向、高度的距离信息)的，以协助机器人定位障碍物，可以直接将物体相对该传感器的方位信息，等价于物体相对于机器人系统或机器人其它组件的方位信息吗？显示是不行的，这中间需要一个转换过程。更具体描述如下：

场景1:雷达与小车
现有一移动式机器人底盘，在底盘上安装了一雷达，雷达相对于底盘的偏移量已知，现雷达检测到一障碍物信息，获取到坐标分别为(x,y,z)，该坐标是以雷达为参考系的，如何将这个坐标转换成以小车为参考系的坐标呢？

场景2：现有一带机械臂的机器人(比如:PR2)需要夹取目标物，当前机器人头部摄像头可以探测到目标物的坐标(x,y,z)，不过该坐标是以摄像头为参考系的，而实际操作目标物的是机械臂的夹具，当前我们需要将该坐标转换成相对于机械臂夹具的坐标，这个过程如何实现？

• 窗口1：第2只乌龟会跟随第1只乌龟运动

roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch（C++实现)
roslaunch turtle_tf2 turtle_tf2_demo.launch（python实现)

1.0 概念 - 作用 - 说明

概念

• tf：TransForm Frame 坐标变换
• 坐标系：ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的
  
  作用
• 在 ROS 中用于实现不同坐标系之间的点或向量的转换

说明
在ROS中坐标变换最初对应的是tf，不过在 hydro 版本开始, tf 被弃用，迁移到 tf2,后者更为简洁高效，tf2对应的常用功能包有：

• tf2_geometry_msgs：可以将ROS消息转换成tf2消息
• tf2：封装了坐标变换的常用消息
• tf2_ros：为 tf2 提供了 roscpp 和 rospy 绑定，封装了坐标变换常用的 API
  官方参考

1.1 坐标 msg 消息

订阅发布模型中数据载体 msg 是一个重要实现，首先需要了解一下，在坐标转换实现中常用的 msg：

• geometry_msgs/TransformStamped：用于传输坐标系相关位置信息
• geometry_msgs/PointStamped：用于传输某个坐标系内坐标点的信息

在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。

1.1.1 geometry_msgs/TransformStamped 坐标系之间位置信息

• 命令行键入：rosmsg info geometry_msgs/TransformStamped，传输坐标系相关位置信息

名词解释

• string frame_id 被参考坐标系(基坐标系)，string child_frame_id 另外坐标系
• geometry_msgs/Quaternion rotation 坐标 A 相对于坐标 B 的欧拉角(三个角)

std_msgs/Header header                     # 头信息
  uint32 seq                                #|-- 序列号
  time stamp                                #|-- 时间戳
  string frame_id                           #|-- 坐标 ID
string child_frame_id                    # 子坐标系的 id
geometry_msgs/Transform transform        # 坐标信息
  geometry_msgs/Vector3 translation        # 偏移(平移)
    float64 x                                #|-- X 方向的偏移量
    float64 y                                #|-- Y 方向的偏移量
    float64 z                                #|-- Z 方向上的偏移量
  geometry_msgs/Quaternion rotation        # 四元数，旋转
    float64 x                              # 偏航，俯仰，翻滚角度上的变化
    float64 y                              #   
    float64 z                                
    float64 w

• 四元数用于表示坐标的相对姿态

1.1.2 geometry_msgs/PointStamped 坐标系内坐标点

• 命令行键入：rosmsg info geometry_msgs/PointStamped，传输某个坐标系内坐标点的信息

std_msgs/Header header                    # 头
  uint32 seq                                #|-- 序号
  time stamp                                #|-- 时间戳
  string frame_id                           #|-- 所属坐标系的 id
geometry_msgs/Point point                # 点坐标
  float64 x                                    #|-- x y z 坐标
  float64 y
  float64 z

1.2 静态坐标变换 —— 功能包 tf01_static

• 静态坐标变换，是指两个坐标系之间的相对位置是固定的。

1.2.0 需求 - 分析 - 流程

需求

现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下：x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？

分析

• 坐标系相对关系，可以通过发布方发布
• 订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出

流程：C++ 与 Python 实现流程一致

• 新建功能包，添加依赖
• 编写发布方实现
• 编写订阅方实现
• 执行并查看结果

1.2.1 新建功能包，添加依赖

• 新建功能包

tf01_static

• 添加依赖

roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs

1.2.2 C++实现

1.2.2.1 发布方 demo01_static_pub.cpp

#include"ros/ros.h"
#include"tf2_ros/static_transform_broadcaster.h"
#include"geometry_msgs/TransformStamped.h"
#include"tf2/LinearMath/Quaternion.h"

/*
    需求： 发布两个坐标系的相对关系
    流程：
        1. 包含头文件
        2. 设置编码 节点初始化 NodeHandle
        3. 创建发布对象
        4. 组织被发布的消息
        5. 发布数据
        6. spin()
*/

int main(int argc, char *argv[])
{
    
    // 2. 设置编码 节点初始化 NodeHandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_pub");
    ros::NodeHandle nh;(不是必须的)

    // 3. 创建发布对象 对应头文件2 —— tf2_ros/static_transform_broadcaster.h 静态坐标转换广播器
    tf2_ros::StaticTransformBroadcaster pub;

    // 4. 组织被发布的消息 对应头文件3 —— geometry_msgs/TransformStamped.h
    geometry_msgs::TransformStamped tfs;// 创建坐标系信息
    tfs.header.stamp = ros::Time::now();
    tfs.header.frame_id = "base_link";// 相对坐标系关系中被参考的那个，基坐标系
    tfs.child_frame_id = "laser";//雷达坐标系
    tfs.transform.translation.x = 0.2;//雷达x偏移值
    tfs.transform.translation.y = 0.0;//雷达y偏移值
    tfs.transform.translation.z = 0.5;//雷达z偏移值

    // 4.1 需要根据欧拉角转换 对应头文件4 —— tf2/LinearMath/Quaternion.h
    tf2::Quaternion qtn; // 创建四元数对象
    // 4.2 向该对象设置欧拉角，这个对象可以将欧拉角转换成四元数
    qtn.setRPY(0,0,0);// 设置偏航值，俯仰值，翻滚值，单位是弧度
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();

    // 5. 发布数据
    pub.sendTransform(tfs);

    // 6. spin()
    ros::spin();

    return 0;
}

1.2.2.2 配置 CMakeLists.txt

add_executable(demo01_static_pub src/demo01_static_pub.cpp)

add_dependencies(demo01_static_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(demo01_static_pub
  ${catkin_LIBRARIES}
)

1.2.2.3 编译检验 demo01_static_pub.cpp

• 编译

Ctrl + Shift + B

检验方式1

• 窗口1：挂起

roscore
cd demo02_ws/
source ./devel/setup.bash
rosrun tf01_static demo01_static_pub

• 窗口2

rostopic list

• 打印该话题下的消息

rostopic echo /tf_static

检验方式2

• 窗口1：图形化设置

rviz

• Fixed Frame 选择 base_link

• 点击左下角 Add，选择 TF
• 红色 x 轴，绿色 y 轴，蓝色 z 轴
  

1.2.2.4 订阅方 demo02_static_sub.cpp

• 核心代码

 // 核心代码 ---- 将 ps 转换成相对于 base_link的坐标点 头文件5 —— tf2_geometry_msgs/tf2_geometry_msgs.h
        geometry_msgs::PointStamped ps_out; 
        /*
            调用buffer(数据存在缓存buffer里面)的转换函数 transform
            参数1: 被转换的坐标点
            参数2: 目标坐标系
            返回值: 输出的坐标点
            注意1: 调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
            注意2: 运行时存在的问题，抛出一个异常 base_link不存在
                  原因:
                     订阅数据是一个耗时操作，可能调用transform转换函数时，
                     坐标系相对关系还没有订阅到，因此出现异常
                  解决:
                     方案1: 在调用转换函数时，执行休眠
                     方案2: 进行异常处理
        */
        ps_out = buffer.transform(ps,"base_link"); // 目标点信息，基参考系，输出转换后的坐标

• 订阅方程序

#include"ros/ros.h"
#include"tf2_ros/transform_listener.h"
#include"tf2_ros/buffer.h"
#include"geometry_msgs/PointStamped.h"
#include"tf2_geometry_msgs/tf2_geometry_msgs.h"

/*
    订阅方: 订阅发布的坐标系相对关系，传入一个坐标系，调用tf实现转换

    流程:
        1. 包含头文件
        2. 编码 初始化 NodeHandle(必须)
        3. 创建订阅对象: ---> 订阅坐标系相对关系
        4. 组织一个坐标点数据
        5. 转换算法，需要调用tf内置实现
        6. 最后输出
*/

int main(int argc, char *argv[])
{
    // 2. 编码 初始化 NodeHandle(必须)
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;

    // 3. 创建订阅对象: ---> 订阅坐标系相对关系 头文件2\3
    // 3.1 创建一个buffer缓存
    tf2_ros::Buffer buffer;
    // 3.2 再创建监听对象(监听对象可以将订阅的数据存入buffer)
    tf2_ros::TransformListener listener(buffer);

    // 4. 组织一个坐标点数据 头文件4
    geometry_msgs::PointStamped ps;
    ps.header.stamp = ros::Time::now();
    ps.header.frame_id = "laser";// 坐标系 障碍物位置相对于雷达坐标系而言
    ps.point.x = 2.0; // 障碍物相对于雷达坐标系的位置
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    
    // 添加休眠 防止订阅方没有订阅到发布方消息
    ros::Duration(2).sleep();
    // 5. 转换算法，需要调用tf内置实现
    ros::Rate rate(1);
    while(ros::ok())
    {
        // 核心代码 ---- 将 ps 转换成相对于 base_link的坐标点 头文件5
        geometry_msgs::PointStamped ps_out; 
        /*
            调用buffer(数据存在缓存buffer里面)的转换函数 transform
            参数1: 被转换的坐标点
            参数2: 目标坐标系
            返回值: 输出的坐标点
            注意1: 调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
            注意2: 运行时存在的问题，抛出一个异常 base_link不存在
                  原因:
                     订阅数据是一个耗时操作，可能调用transform转换函数时，
                     坐标系相对关系还没有订阅到，因此出现异常
                  解决:
                     方案1: 在调用转换函数时，执行休眠
                     方案2: 进行异常处理
        */
        ps_out = buffer.transform(ps,"base_link"); // 目标点信息，基参考系，输出转换后的坐标

        // 6. 最后输出 
        ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
                    ps_out.point.x,
                    ps_out.point.y,
                    ps_out.point.z,
                    ps.header.frame_id.c_str()
                    );
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

1.2.2.5 配置 CMakeLists.txt

add_executable(demo02_static_sub src/demo02_static_sub.cpp)

add_dependencies(demo02_static_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(demo02_static_sub
  ${catkin_LIBRARIES}
)

1.2.2.6 发布方与订阅方 编译执行

• 注意：在订阅方程序中增加延时，保证先发布，再订阅，防止报错
• 编译

Ctrl + Shift + B

• 窗口1

roscore

• 启动发布方 窗口2

cd demo02_ws/
source ./devel/setup.bash
rosrun tf01_static demo01_static_pub

• 启动订阅方 窗口3

cd demo02_ws/
source ./devel/setup.bash
rosrun tf01_static demo02_static_sub

1.2.3 Python实现

1.2.3.1 发布方 demo01_static_pub_p.py

#! /usr/bin/env python

"""
    发布方: 发布两个坐标系的相对关系(车辆底盘 --- base_link 和 雷达 --- laser)
    流程:
        1. 导包
        2. 初始化节点
        3. 创建发布对象
        4. 组织被发布的数据
        5. 发布数据
        6. spin()
"""

import rospy
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped

if __name__ == "__main__":
     
    # 2. 初始化节点
    rospy.init_node("start_pub_p")
    # 3. 创建发布对象 头文件2
    pub = tf2_ros.StaticTransformBroadcaster()
    # 4. 组织被发布的数据 头文件4
    tfs = TransformStamped()
    # 4.1 header
    tfs.header.stamp = rospy.Time.now()
    tfs.header.frame_id = "base_link"
    # 4.2 child frame
    tfs.child_frame_id = "laser"
    # 4.3 相对关系(偏移 与 四元数)
    tfs.transform.translation.x = 0.2
    tfs.transform.translation.y = 0.0
    tfs.transform.translation.z = 0.5
    # 4.4 先从欧拉角转换成四元数 头文件3
    qtn = tf.transformations.quaternion_from_euler(0,0,0)
    # 4.5 再设置四元数
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]

    # 5. 发布数据
    pub.sendTransform(tfs)
    # 6. spin()
    rospy.spin()

1.2.3.2 订阅方 demo02_static_sub_p.py

#! /usr/bin/env python

"""
    订阅方: 订阅坐标变换消息，传入被转换的坐标，调用转换算法

    流程:
        1. 导包
        2. 初始化
        3. 创建订阅对象
        4. 组织被转换的坐标点
        5. 转换逻辑实现，调用tf封装的算法
        6. 输出结果
"""

import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs

if __name__ == "__main__":

    # 2. 初始化
    rospy.init_node("static_sub_p")

    # 3. 创建订阅对象
    # 3.1 创建一个缓存对象 头文件2
    buffer = tf2_ros.Buffer()
    # 3.2 创建订阅对象(将缓存传入)
    sub = tf2_ros.TransformListener(buffer)

    # 4. 组织被转换的坐标点 头文件3
    ps = tf2_geometry_msgs.PointStamped()
    ps.header.stamp = rospy.Time.now()
    ps.header.frame_id = "laser"
    ps.point.x = 2.0
    ps.point.y = 3.0
    ps.point.z = 5.0

    # 5. 转换逻辑实现，调用tf封装的算法
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        # 使用try防止
        try:
            # 转换实现
        """
            参数1: 被转换的坐标点
            参数2: 目标坐标系
            返回值: 转换后的坐标点

            PS:
            问题: 抛出异常 base_link 不存在
            原因: 转换函数调用时，可能还没有订阅到坐标系的相对关系
            解决: try 捕获异常，并处理
        """
            ps_out = buffer.transform(ps,"base_link")
            # 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.logwarn("错误提示:%s",e) 
        
        rate.sleep()

1.2.4 x - 翻滚 y - 俯仰 z - 偏航(重要!!!)

• 当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y 俯仰角度、z 偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，摄像头相对于基坐标系位置。
• rosrun tf2_ros static_transform_publisher 0.1 0.0 0.3 0 0 0 /base_link /camera
• rviz：依次是z - 偏航角(Yaw)，y - 俯仰角(Pitch)，x 翻滚角(Roll)
• X轴：红色(翻滚角) 正值 —— 顺时针转动；负值 —— 逆时针转动
• Y轴：绿色(俯仰角)
• Z轴：蓝色(偏航角)
  
  

1.2.5 补充讲解

1.2.5.1 补充1

当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x翻滚角度、y俯仰角度、z偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下:

• rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度
  x翻滚角度 父级坐标系 子级坐标系
• 例如：rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser

也建议使用该种方式直接实现静态坐标系相对信息发布。

1.2.5.2 补充2

可以借助于rviz显示坐标系关系，具体操作：

• 新建窗口输入命令：rviz
• 在启动的 rviz 中设置Fixed Frame 为 base_link
• 点击左下的 add 按钮，在弹出的窗口中选择 TF 组件，即可显示坐标关系

1.3 动态坐标变换 —— 功能包 tf02_dynamic

1.3.0 需求 - 分析 - 流程

所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。

需求描述

• 启动 turtlesim_node，该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。

实现分析

• 乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
• 订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
• 将 pose 信息转换成 坐标系相对信息并发布

实现流程：C++ 与 Python 实现流程一致

• 新建功能包，添加依赖
• 创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)
• 创建坐标相对关系订阅方
• 编译执行

1.3.1 新建功能包，添加依赖

• 新建功能包

tf02_dynamic

• 添加依赖

roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim

1.3.2 C++实现

1.3.2.0 预准备 - 获取话题和消息

• 话题：/turtle1/pose 消息类型：turtlesim/Pose（类似于Person.msg）
• 启动窗口1

roscore

• 启动窗口2 —— 打开乌龟节点

rosrun turtlesim turtlesim_node

• 启动窗口3 —— 获取话题 /turtle1/pose

rostopic list

启动窗口4 —— 获取消息格式 turtlesim/Pose

rostopic info /turtle1/pose

• 启动窗口5 —— 获取消息的具体格式

rosmsg info turtlesim/Pose

float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

1.3.2.1 发布方 demo01_dynamic_pub.cpp

#include"ros/ros.h"
#include"turtlesim/Pose.h"
// 创建发布对象
#include"tf2_ros/transform_broadcaster.h"
// 组织发布数据
#include"geometry_msgs/TransformStamped.h"
// 坐标系四元数
#include"tf2/LinearMath/Quaternion.h"
/*
    发布方: 需要订阅乌龟的位姿信息, 转换成相对于窗体的坐标关系，并发布
    准  备: 
        话题： /turtle1/pose
        消息： turtlesim/Pose

    流程:
        1. 包含头文件
        2. 设置编码，初始化， NodeHandle
        3. 创建订阅对象，订阅/turtle1/pose
        4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注)
        5. spin()
*/

// 给传进回调函数的数据定义范型 头文件2
void doPose(const turtlesim::Pose::ConstPtr& pose){
    // 获取位姿信息，转换成坐标系相对关系(核心)，并发布
    // a. 创建发布对象 头文件3
    static tf2_ros::TransformBroadcaster pub;

    // b. 组织被发布的数据 头文件4
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world"; // 相对于世界坐标系
    ts.header.stamp = ros::Time::now();
    ts.child_frame_id = "turtle1";
    // 坐标系偏移量
    ts.transform.translation.x = pose->x;
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0; // z一直是零（2D）
    // 坐标系四元数 头文件5
    /*
        位姿信息中没有四元数，但是有偏航角度，又已知乌龟时2D，没有翻滚和俯仰角，所以得出乌龟的
        欧拉角: 0 0 theta
    */
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    ts.transform.rotation.x = qtn.getX();
    ts.transform.rotation.y = qtn.getY();
    ts.transform.rotation.z = qtn.getZ();
    ts.transform.rotation.w = qtn.getW();

    // c. 发布
    pub.sendTransform(ts);
}

int main(int argc, char *argv[])
{
    // 2. 设置编码，初始化， NodeHandle
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;

    // 3. 创建订阅对象，订阅/turtle1/pose
    ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);

    // 4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注)
    // 5. spin()
    ros::spin();
    return 0;
}

1.3.2.2 配置 CMakeLists.txt

add_executable(demo01_dynamic_pub src/demo01_dynamic_pub.cpp)

add_dependencies(demo01_dynamic_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(demo01_dynamic_pub
  ${catkin_LIBRARIES}
)

1.3.2.3 订阅方 demo02_dynamic_sub.cpp(类似 demo02_static.sub)

#include"ros/ros.h"
#include"tf2_ros/transform_listener.h"
#include"tf2_ros/buffer.h"
#include"geometry_msgs/PointStamped.h"
#include"tf2_geometry_msgs/tf2_geometry_msgs.h"

/*
    订阅方: 订阅发布的坐标系相对关系，传入一个坐标系，调用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::NodeHandle nh;

    // 3. 创建订阅对象: ---> 订阅坐标系相对关系 头文件2\3
    // 3.1 创建一个buffer缓存
    tf2_ros::Buffer buffer;
    // 3.2 再创建监听对象(监听对象可以将订阅的数据存入buffer)
    tf2_ros::TransformListener listener(buffer);

    // 4. 组织一个坐标点数据 头文件4
    geometry_msgs::PointStamped ps;
    // 参考的坐标系
    ps.header.frame_id = "turtle1";// 坐标系 物体位置相对于乌龟坐标系而言
    // 时间戳
    ps.header.stamp = ros::Time(0.0);
    
    ps.point.x = 2.0; // 物体相对于乌龟坐标系的位置
    ps.point.y = 3.0;
    ps.point.z = 5.0;
    
    // 添加休眠 防止订阅方没有订阅到发布方消息(这个方法不建议用，最好使用try方法
    // ros::Duration(2).sleep();
    // 5. 转换算法，需要调用tf内置实现
    ros::Rate rate(1);
    while(ros::ok())
    {
        // 核心代码 ---- 将 ps 转换成相对于 base_link的坐标点 头文件5
        geometry_msgs::PointStamped ps_out; 
        /*
            调用buffer(数据存在缓存buffer里面)的转换函数 transform
            参数1: 被转换的坐标点
            参数2: 目标坐标系
            返回值: 输出的坐标点
            注意1: 调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
            注意2: 运行时存在的问题，抛出一个异常 base_link不存在
                  原因:
                     订阅数据是一个耗时操作，可能调用transform转换函数时，
                     坐标系相对关系还没有订阅到，因此出现异常
                  解决:
                     方案1: 在调用转换函数时，执行休眠
                     方案2: 进行异常处理
        */
       try
       {
            ps_out = buffer.transform(ps,"world"); // 目标点信息，基参考系，输出转换后的坐标 头文件5r

            // 6. 最后输出 
            ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
                        ps_out.point.x,
                        ps_out.point.y,
                        ps_out.point.z,
                        ps.header.frame_id.c_str()
                        );
       }
       catch(const std::exception& e)
       {
           ROS_INFO("异常消息:%s",e.what());
       }
        
        rate.sleep();
        ros::spinOnce();
    }
    
    return 0;
}

1.3.2.4 配置 CMakeLists.txt

add_executable(demo02_dynamic_sub src/demo02_dynamic_sub.cpp)

add_dependencies(demo02_dynamic_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(demo02_dynamic_sub
  ${catkin_LIBRARIES}
)

1.3.2.5 编译运行

• 编译

Ctrl + Shift + B

• 启动窗口1

roscore

• 启动窗口2

rosrun turtlesim turtlesim_node 

• 启动发布方 窗口3

cd demo02_ws/
source ./devel/setup.bash
rosrun tf02_dynamic demo01_dynamic_pub

• 启动订阅方 窗口4

cd demo02_ws/
source ./devel/setup.bash
rosrun tf02_dynamic demo02_dynamic_sub 

• 启动窗口5：启动乌龟节点，使用键盘控制乌龟运动，对应转换的坐标值也在变化

cd demo02_ws/
source ./devel/setup.bash
rosrun turtlesim turtle_teleop_key

1.3.3 Python实现

1.3.3.1 发布方 demo01_dynamic_sub_p.py

roscore
rosrun turtlesim turtlesim_node
cd demo02_ws/
source ./devel/setup.bash
rosrun tf02_dynamic demo01_dynamic_pub_p.py

/rosout
/rosout_agg
/tf
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose

rostopic echo /tf 查看具体信息

ransforms:
header:
seq: 0
stamp:
secs: 1658287751
nsecs: 917598485
frame_id: “world”
child_frame_id: “turtle1”
transform:
translation:
x: 5.544444561004639
y: 5.544444561004639
z: 0.0
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0

#! /usr/bin/env python

"""
    发布方: 需要订阅乌龟的位姿信息, 转换成相对于窗体的坐标关系，并发布
    准  备: 
        话题： /turtle1/pose
        消息： turtlesim/Pose

    流程:
        1. 包含头文件
        2. 初始化ROS节点
        3. 创建订阅对象，订阅/turtle1/pose
        4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注)
        5. spin()
"""

import rospy
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
def doPose(pose):
    # 1. 创建发布坐标系相对关系的对象 头文件3
    pub = tf2_ros.TransformBroadcaster()
    # 2. 将 pose 转换成 坐标系相对关系消息 头文件4
    ts = TransformStamped()
    ts.header.frame_id = "world"
    ts.header.stamp = rospy.Time.now()
    ts.child_frame_id = "turtle1"
    
    # 2.1 子级坐标系相对于父级坐标系的偏移量
    ts.transform.translation.x = pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0

    # 2.2 四元数 头文件5
    # 从欧拉角转换四元数
    """
        乌龟是2D，不存在 x 上的翻滚和 y 上的俯仰，只有 z 上的偏航
        0 0 pose.theta
    """
    qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]

    # 3. 发布
    pub.sendTransform(ts)

if __name__ == "__main__":

    # 2. 初始化ROS节点
    rospy.init_node("dynamic_pub_p")
    # 3. 创建订阅对象 头文件2
    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size = 100)# /tur是话题名称

    # 4. 回调函数处理订阅信息
    # 5. spin()
    rospy.spin()

1.3.3.2 订阅方 demo02_dynamic_sub_p.py

roscore
cd demo02_ws/
rosrun turtlesim turtlesim_node
source ./devel/setup.bash
rosrun tf02_dynamic demo01_dynamic_pub_p.py
rosrun tf02_dynamic demo02_dynamic_sub_p.py
控制乌龟运动
rosrun turtlesim turtle_teleop_key

#! /usr/bin/env python

"""
    订阅方: 订阅坐标变换消息，传入被转换的坐标，调用转换算法

    流程:
        1. 导包
        2. 初始化
        3. 创建订阅对象
        4. 组织被转换的坐标点
        5. 转换逻辑实现，调用tf封装的算法
        6. 输出结果
"""

import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs

if __name__ == "__main__":

    # 2. 初始化
    rospy.init_node("dynamic_sub_p")

    # 3. 创建订阅对象
    # 3.1 创建一个缓存对象 头文件2
    buffer = tf2_ros.Buffer()
    # 3.2 创建订阅对象(将缓存传入)
    sub = tf2_ros.TransformListener(buffer)

    # 4. 组织被转换的坐标点 头文件3
    ps = tf2_geometry_msgs.PointStamped()
    # 时间戳--0
    ps.header.stamp = rospy.Time()
    # 坐标系
    ps.header.frame_id = "turtle1"
    ps.point.x = 2.0
    ps.point.y = 3.0
    ps.point.z = 5.0

    # 5. 转换逻辑实现，调用tf封装的算法
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        # 使用try防止
        try:
            # 转换实现
        """
            参数1: 被转换的坐标点
            参数2: 目标坐标系
            返回值: 转换后的坐标点

            PS:
            问题: 抛出异常 base_link 不存在
            原因: 转换函数调用时，可能还没有订阅到坐标系的相对关系
            解决: try 捕获异常，并处理
        """
            ps_out = buffer.transform(ps,"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.logwarn("错误提示:%s",e) 
        
        rate.sleep()