第 5 章 ROS 常用组件 2 —— TF 坐标变换_多坐标变换 tf03_tfs(重难点)

01 多坐标变换

1.1 需求 - 分析 - 流程

需求描述：

• 现有坐标系统，父级坐标系统 world，下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于world 的关系是已知的，求 son1 原点在 son2 中的坐标，又已知在 son1 中一点的坐标，要求求出该点在 son2 中的坐标。

实现分析：

• 首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
• 然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换
• 最后，还要实现坐标点的转换

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

• 新建功能包，添加依赖
• 创建坐标相对关系发布方(需要发布两个坐标相对关系)
• 创建坐标相对关系订阅方
• 编译执行

1.2 新建功能包，添加依赖 —— 功能包 tf03_tfs

• 新建功能包

tf01_static

• 添加依赖

roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim

1.3 程序实现

1.3.1 发布方 —— tfs_c.launch

• 为了方便，使用静态坐标变换发布

<launch>
    <!-- 发布 son1 相对于 world 以及 son2 相对于 world 的关系 -->
    <node pkg = "tf2_ros" type = "static_transform_publisher" name = "son1" args = "5 0 0 0 0 0 /world /son1" output = "screen" />
    <node pkg = "tf2_ros" type = "static_transform_publisher" name = "son2" args = "3 0 0 0 0 0 /world /son2" output = "screen" />
</launch> 

• 启动窗口1

cd demo02_ws/
roslaunch tf03_tfs tfs_c.launch

• 启动窗口2

rviz

• fixed frame 选择是 world，选择 Add 中 TF

1.3.2 编译运行

• 编译

Ctrl + Shift + B

• 启动窗口 1

roscore

• 启动窗口 2

cd demo02_ws/
source ./devel/setup.bash
roslaunch tf03_tfs tfs_c.launch(先启动)

• 启动窗口 3

cd demo02_ws/
source ./devel/setup.bash
rosrun tf03_tfs demo01_tfs_sub(开新的窗口)

son1 相对于 son2 的信息：父级: son2，子级: son1 偏移量(2.00,0.00,0.00)
son1 相对于 son2 的信息：父级: son2，子级: son1

• 启动窗口 4

rviz

1.3.3 订阅方

1.3.3.1 C++实现 —— demo01_tfs.cpp

#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"
#include"geometry_msgs/TransformStamped.h"

/*
    订阅方实现: 1. 计算 son1 与 son2 的相对关系 2. 计算 son1 的某个坐标点在 son2 中的坐标值
    流程:
        1. 包含头文件
        2. 编码，初始化 NodeHandle
        3. 创建订阅对象
        4. 编写解析逻辑
        5. spinOnce()
*/

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

    // 3. 创建订阅对象 头文件2/3
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    // 4. 编写解析逻辑

    // 创建坐标点 头文件4/5
    geometry_msgs::PointStamped psAtSon1;

    psAtSon1.header.stamp = ros::Time::now();
    psAtSon1.header.frame_id = "son1";
    psAtSon1.point.x = 1.0;
    psAtSon1.point.y = 2.0;
    psAtSon1.point.z = 3.0;

    ros::Rate rate(10);
    while(ros::ok())
    {
        // 核心
        try
        {
            // 1. 计算 son1 与 son2 的相对关系
            /*
                A相对于B的坐标系关系
                参数1: 目标坐标系 B
                参数2: 源坐标系 A
                参数3: ros::Time(0)取间隔最短的两个坐标关系帧来计算相对关系
                返回值: geometry_msgs::TransformStamped 源相对于目标坐标系的相对关系
            */
            geometry_msgs::TransformStamped son1ToSon2 =  buffer.lookupTransform("son2","son1",ros::Time(0)); 
            // 目标坐标系，源坐标系，时间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,
                    son1ToSon2.transform.translation.y,
                    son1ToSon2.transform.translation.z
                    ); 
            // 2. 计算 son1 的某个坐标点在 son2 中的坐标值
            geometry_msgs::PointStamped psAtSon2 = buffer.transform(psAtSon1,"son2"); 
            // 前者是被转换的坐标点,后者是转换的坐标系
            ROS_INFO("坐标点在 Son2 中的值(%.2f,%.2f,%.2f)",
                    psAtSon2.point.x,
                    psAtSon2.point.y,
                    psAtSon2.point.z
                    );
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示: %s",e.what());
        }
        
        rate.sleep();
        ros::spinOnce();
    }
    
    // 5. spinOnce()

    return 0;
}

1.3.3.2 配置 CMakeLists.txt

add_executable(demo01_tfs src/demo01_tfs.cpp)

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

target_link_libraries(demo01_tfs
  ${catkin_LIBRARIES}
)

1.3.3.3 Python实现 —— demo01_tfs_p.py

转换后的坐标:(3.00,2.00,3.00)，参考的坐标系: son2 父级坐标系: son2,子级坐标系: son1, 偏移量(2.00,0.00,0.00)

#! /usr/bin/env python
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped

if __name__ == "__main__":

    # 2. 初始化
    rospy.init_node("tfs_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.stamp = rospy.Time.now() # launch使用的是静态坐标变换

    # 坐标系
    ps.header.frame_id = "son1"
    ps.point.x = 1.0
    ps.point.y = 2.0
    ps.point.z = 3.0

    # 5. 转换逻辑实现，调用tf封装的算法
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        # 使用try防止
        try:
            # ------ 需要计算 son1 相对于 son2 的坐标关系  头文件4
            """
                参数1: 目标坐标系
                参数2: 源坐标系
                参数3: rospy.Time(0) --- 取时间间隔最近的两个坐标系帧(son1相对world,与son2相对world)计算
                返回值: son1 与 son2坐标系关系
            """
            ts = buffer.lookup_transform("son2","son1",rospy.Time(0))
            rospy.loginfo("父级坐标系: %s,子级坐标系: %s, 偏移量(%.2f,%.2f,%.2f)",
                        ts.header.frame_id,
                        ts.child_frame_id,
                        ts.transform.translation.x,
                        ts.transform.translation.y,
                        ts.transform.translation.z
                        )
            # 转换实现
            ps_out = buffer.transform(ps,"son2") # 被转换的坐标点，目标坐标系的名称
            # 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.3.3.4 配置 CMakeLists.txt

• 启动窗口1

roscore

• 启动窗口2

source ./devel/setup.bash
roslaunch tf03_tfs tfs_c.launch 

• 启动窗口3

rosrun tf2_tools view_frames.py

• 在 vscode 终端里打开：ctrl+`

02 坐标系关系查看

在机器人系统中，涉及的坐标系有多个，为了方便查看，ros 提供了专门的工具，可以用于生成显示坐标系关系的 pdf 文件，该文件包含树形结构的坐标系图谱。

2.1 准备

• 首先调用rospack find tf2_tools查看是否包含该功能包，如果没有，请使用如下命令安装

sudo apt install ros-noetic-tf2-tools

2.2 使用

2.2.1 生成 pdf 文件

• 启动坐标系广播程序之后，运行如下命令：

rosrun tf2_tools view_frames.py

• 会产生类似于下面的日志信息

[INFO] [1592920556.827549]: Listening to tf data during 5 seconds...
[INFO] [1592920561.841536]: Generating graph in frames.pdf file...

• 查看当前目录会生成一个 frames.pdf 文件

2.2.2 查看文件

• 可以直接进入目录打开文件，或者调用命令查看文件：evince frames.pdf( 查看 pdf)