前言

📢本系列将依托赵虚左老师的ROS课程，写下自己的一些心得与笔记。
📢课程链接:https://www.bilibili.com/video/BV1Ci4y1L7ZZ
📢讲义链接:http://www.autolabor.com.cn/book/ROSTutorials/index.html
📢 文章可能存在疏漏的地方，恳请大家指出。

1. 多态坐标变换

需求描述:

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

实现分析:

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

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

• 新建功能包，添加依赖

• 创建坐标相对关系发布方(需要发布两个坐标相对关系)

• 创建坐标相对关系订阅方

• 执行

1.1 发布方

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

<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>

启动

roslaunch tf2_test tf2_test_two.launch 

在rviz中查看

1.2 订阅方(C++)

#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"
/*

需求:
    现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，
    son1 相对于 world，以及 son2 相对于 world 的关系是已知的，
    求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
实现流程:
    1.包含头文件
    2.初始化 ros 节点
    3.创建 ros 句柄
    4.创建 TF 订阅对象
    5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标
      解析 son1 中的点相对于 son2 的坐标
    6.spin

*/
int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"tfs");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 TF 订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);
    // 5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标
    //   解析 son1 中的点相对于 son2 的坐标

    //创建坐标点
    geometry_msgs::PointStamped pson1;
    pson1.header.frame_id = "son1";
    pson1.header.stamp = ros::Time::now();
    pson1.point.x  = 1.0;
    pson1.point.y  = 2.0;
    pson1.point.z  = 3.0;
    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
          //1.计算son1与son2之间的相对关系
        geometry_msgs::TransformStamped son1toson2= buffer.lookupTransform("son1","son2",ros::Time(0));
        ROS_INFO("son1相对于son2的相对关系:父级:%s,子级:%s,偏移量:%0.2f,%0.2f,%0.2f",
            son1toson2.header.frame_id.c_str(),
            son1toson2.child_frame_id.c_str(),
            son1toson2.transform.translation.x,
            son1toson2.transform.translation.y,
            son1toson2.transform.translation.z);
          //2.计算son1中的某个目标点在son2中的坐标值
         geometry_msgs::PointStamped pson2;
         pson2 = buffer.transform(pson1,"son2");
         ROS_INFO("坐标点在son2中的值%0.2f,%0.2f,%0.2f",
                                                                            pson2.point.x,
                                                                            pson2.point.y,
                                                                            pson2.point.z);
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示:%s",e.what());
        }
        
        rate.sleep();
        ros::spinOnce();
    }
    
    // 6.spin
    return 0;
}

PS:
lookupTransform的参数解释

参数	解释
参数1	目标坐标系(son2)
参数2	源坐标系(son1)
参数3	ros::Time(0) 取间隔最短的两个坐标关系帧计算相对关系
返回值	geometry_msgs::TransformStamped源相对于目标坐标系的相对关系

计算son1中的某个目标点在son2中的坐标值(3,2,3)

计算son1与son2之间的相对关系

[ INFO] [1673275545.749496126]: son1相对于son2的相对关系:父级:son1,子级:son2,偏移量:-2.00,0.00,0.00
[ INFO] [1673275545.749576950]: 坐标点在son2中的值3.00,2.00,3.00
[ INFO] [1673275545.849524384]: son1相对于son2的相对关系:父级:son1,子级:son2,偏移量:-2.00,0.00,0.00
[ INFO] [1673275545.849619936]: 坐标点在son2中的值3.00,2.00,3.00
[ INFO] [1673275545.949552460]: son1相对于son2的相对关系:父级:son1,子级:son2,偏移量:-2.00,0.00,0.00
[ INFO] [1673275545.949686138]: 坐标点在son2中的值3.00,2.00,3.00
[ INFO] [1673275546.049589769]: son1相对于son2的相对关系:父级:son1,子级:son2,偏移量:-2.00,0.00,0.00
[ INFO] [1673275546.049734242]: 坐标点在son2中的值3.00,2.00,3.00
[ INFO] [1673275546.149552228]: son1相对于son2的相对关系:父级:son1,子级:son2,偏移量:-2.00,0.00,0.00
[ INFO] [1673275546.149697011]: 坐标点在son2中的值3.00,2.00,3.00
[ INFO] [1673275546.249640250]: son1相对于son2的相对关系:父级:son1,子级:son2,偏移量:-2.00,0.00,0.00
[ INFO] [1673275546.249781671]: 坐标点在son2中的值3.00,2.00,3.00
[ INFO] [1673275546.349580365]: son1相对于son2的相对关系:父级:son1,子级:son2,偏移量:-2.00,0.00,0.00
[ INFO] [1673275546.349704334]: 坐标点在son2中的值3.00,2.00,3.00

1.3 订阅方(python)

发布方都是用launch文件进行,所以不再重复.python版本的订阅方实现如下

#!/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
import tf2_ros
from geometry_msgs.msg import TransformStamped
from tf2_geometry_msgs import PointStamped

if __name__ == "__main__":

    # 2.初始化 ROS 节点
    rospy.init_node("frames_sub_p")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():

        try:
        # 4.调用 API 求出 son1 相对于 son2 的坐标关系
            #lookup_transform(self, target_frame, source_frame, time, timeout=rospy.Duration(0.0)):
            tfs = buffer.lookup_transform("son2","son1",rospy.Time(0))
            rospy.loginfo("son1 与 son2 相对关系:")
            rospy.loginfo("父级坐标系:%s",tfs.header.frame_id)
            rospy.loginfo("子级坐标系:%s",tfs.child_frame_id)
            rospy.loginfo("相对坐标:x=%.2f, y=%.2f, z=%.2f",
                        tfs.transform.translation.x,
                        tfs.transform.translation.y,
                        tfs.transform.translation.z,
            )
        # 5.创建一依赖于 son1 的坐标点，调用 API 求出该点在 son2 中的坐标
            point_source = PointStamped()
            point_source.header.frame_id = "son1"
            point_source.header.stamp = rospy.Time.now()
            point_source.point.x = 1
            point_source.point.y = 1
            point_source.point.z = 1

            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,
                        point_target.point.y,
                        point_target.point.z
            )

        except Exception as e:
            rospy.logerr("错误提示:%s",e)


        rate.sleep()
    # 6.spin    
    # rospy.spin()

2. 坐标系关系查看

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

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

sudo apt install ros-<ROS版本号>-tf2-tools

以多坐标变换的launch文件为例,启动launch文件

roslaunch tf2_test tf2_test_two.launch 

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

rosrun tf2_tools view_frames.py

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

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

查看当前目录会生成一个 frames.pdf 文件
可以直接进入目录打开文件，或者调用命令查看文件:evince frames.pdf

3. TF坐标变换实操(C++)

实现分析:

乌龟跟随实现的核心，是乌龟A和B都要发布相对世界坐标系的坐标信息，然后，订阅到该信息需要转换获取A相对于B坐标系的信息，最后，再生成速度信息，并控制B运动。

• 启动乌龟显示节点
• 在乌龟显示窗体中生成一只新的乌龟(需要使用服务)
• 编写两只乌龟发布坐标信息的节点
• 编写订阅节点订阅坐标信息并生成新的相对关系生成速度信息

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

• 新建功能包，添加依赖

• 编写服务客户端，用于生成一只新的乌龟

• 编写发布方，发布两只乌龟的坐标信息

• 编写订阅方，订阅两只乌龟信息，生成速度信息并发布

• 运行

3.1准备

1.新建一个功能包(tf2_turtle),包含以下依赖 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim

2.创建一个launch文件(start_tf_turtle.launch)

3.2 生成新的乌龟

实现生成乌龟龟的功能(spawn),此项任务在之前【ROS】—— ROS通信机制——实践与练习（六）已经实现,直接将代码复制,稍作修改即可.

spawn_new_turtle.cpp

/*
    生成一只小乌龟
    准备工作:
        1.服务话题 /spawn
        2.服务消息类型 turtlesim/Spawn
        3.运行前先启动 turtlesim_node 节点

    实现流程:
        1.包含头文件
          需要包含 turtlesim 包下资源，注意在 package.xml 配置
        2.初始化 ros 节点
        3.创建 ros 句柄
        4.创建 service 客户端
        5.等待服务启动
        6.发送请求
        7.处理响应

*/

#include "ros/ros.h"
#include "turtlesim/Spawn.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"set_turtle");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 service 客户端
    ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/spawn");
    // 5.等待服务启动
    // client.waitForExistence();
    ros::service::waitForService("/spawn");
    // 6.发送请求
    turtlesim::Spawn spawn;
    spawn.request.x = 1.0;
    spawn.request.y = 4.0;
    spawn.request.theta = 1.57;
    spawn.request.name = "turtle2";
    bool flag = client.call(spawn);
    // 7.处理响应结果
    if (flag)
    {
        ROS_INFO("新的乌龟生成,名字:%s",spawn.response.name.c_str());
    } else {
        ROS_INFO("乌龟生成失败！！！");
    }


    return 0;
}

将launch文件更改如下:

<launch>
    <!-- 启动乌龟GUI-->
    <node pkg="turtlesim" type = "turtlesim_node" name ="turtle1" output = "screen"/>
    <!-- 生成新的一只乌龟-->
    <node pkg="tf2_turtle" type = "spawn_new_turtle" name ="turtle2" output = "screen"/>
</launch>

启动launch文件,出现如下则运行成功.

3.3 增加键盘控制

若相对键盘控制进行修改(控制另一只乌龟),可以参考【ROS】—— ROS运行管理 ——元功能包与launch文件（八）或者使用rostopic pub参考【ROS】——常用命令（五）

<launch>
    <!-- 启动乌龟GUI-->
    <node pkg="turtlesim" type = "turtlesim_node" name ="turtle1" output = "screen"/>
    <!-- 键盘控制-->
    <node pkg="turtlesim" type = "turtle_teleop_key" name ="teleop_key" output = "screen"/>
    <!-- 生成新的一只乌龟-->
    <node pkg="tf2_turtle" type = "spawn_new_turtle" name ="turtle2" output = "screen"/>
</launch>

3.4 发布方(发布两只乌龟的坐标信息)

订阅乌龟的位姿信息，然后再转换成坐标信息并发布
类似于【ROS】—— ROS常用组件_TF坐标变换_静态坐标变换与动态坐标变换(十)中的坐标变换,只不过此时需要发布两只乌龟的坐标信息.复制代码的方法可行,但却降低了代码的复用率.对此,我们有以下实现思路:

1.节点只编写一个
2.此节点需要启动两次
3.节点启动时动态传参:第一次启动后传递turtle1,第二次启动传递turtle2.

tf2_pub_turtle.cpp

/*  
    该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息

    注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
          其他的话题名称和实现逻辑都是一样的，
          所以我们可以将所需的命名空间通过 args 动态传入

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.解析传入的命名空间
        4.创建 ros 句柄
        5.创建订阅对象
        6.回调函数处理订阅的 pose 信息
            6-1.创建 TF 广播器
            6-2.将 pose 信息转换成 TransFormStamped
            6-3.发布
        7.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"
//保存乌龟名称
std::string turtle_name;


void doPose(const turtlesim::Pose::ConstPtr& pose){
    //  6-1.创建 TF 广播器 ---------------------------------------- 注意 static
    static tf2_ros::TransformBroadcaster broadcaster;
    //  6-2.将 pose 信息转换成 TransFormStamped
    geometry_msgs::TransformStamped tfs;
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();
    //传参关键点
    tfs.child_frame_id = turtle_name;
    //偏移量
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0;
    //四元数
    tf2::Quaternion qtn;
    qtn.setRPY(0,0,pose->theta);
    tfs.transform.rotation.x = qtn.getX();
    tfs.transform.rotation.y = qtn.getY();
    tfs.transform.rotation.z = qtn.getZ();
    tfs.transform.rotation.w = qtn.getW();
    //  6-3.发布
    broadcaster.sendTransform(tfs);

} 

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"pub_tf");
    // 3.解析传入的命名空间
    if (argc != 2)
    {
        ROS_ERROR("请传入正确的参数");
    } else {
        turtle_name = argv[1];
        ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());
    }

    // 4.创建 ros 句柄
    ros::NodeHandle nh;
    // 5.创建订阅对象
    ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",1000,doPose);
    //     6.回调函数处理订阅的 pose 信息
    //         6-1.创建 TF 广播器
    //         6-2.将 pose 信息转换成 TransFormStamped
    //         6-3.发布
    // 7.spin
    ros::spin();
    return 0;
}

launch文件如下

<launch>
    <!-- 启动乌龟GUI-->
    <node pkg="turtlesim" type = "turtlesim_node" name ="turtle1" output = "screen"/>
    <!-- 键盘控制-->
    <node pkg="turtlesim" type = "turtle_teleop_key" name ="teleop_key" output = "screen"/>
    <!-- 生成新的一只乌龟-->
    <node pkg="tf2_turtle" type = "spawn_new_turtle" name ="turtle2" output = "screen"/>
    <!-- 启动两个乌龟相对世界的坐标关系的发布-->
    <node pkg="tf2_turtle" type = "tf2_pub_turtle" name ="pub1" args="turtle1" output = "screen"/>
    <node pkg="tf2_turtle" type = "tf2_pub_turtle" name ="pub2" args="turtle2" output = "screen"/>
</launch>

启动launch文件后,再启动rviz,得到以下结果,则运行成功(三个坐标系)

3.5 订阅方(解析坐标信息并生成速度信息)

1. 订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
2. 将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布

/*  
    订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
    将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布

    实现流程:
        1.包含头文件
        2.初始化 ros 节点
        3.创建 ros 句柄
        4.创建 TF 订阅对象
        5.处理订阅到的 TF
        6.spin

*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ros 节点
    ros::init(argc,argv,"sub_TF");
    // 3.创建 ros 句柄
    ros::NodeHandle nh;
    // 4.创建 TF 订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);
    // 5.处理订阅到的 TF

    // 需要创建发布 /turtle2/cmd_vel 的 publisher 对象

    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",1000);

    ros::Rate rate(10);
    while (ros::ok())
    {
        try
        {
            //5-1.先获取 turtle1 相对 turtle2 的坐标信息
            geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));

            //5-2.根据坐标信息生成速度信息 -- geometry_msgs/Twist.h
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));
            twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);

            //5-3.发布速度信息 -- 需要提前创建 publish 对象
            pub.publish(twist);
        }
        catch(const std::exception& e)
        {
            // std::cerr << e.what() << '\n';
            ROS_INFO("错误提示:%s",e.what());
        }



        rate.sleep();
        // 6.spin
        ros::spinOnce();
    }

    return 0;
}

最后的launch文件

<launch>
    <!-- 启动乌龟GUI-->
    <node pkg="turtlesim" type = "turtlesim_node" name ="turtle1" output = "screen"/>
    <!-- 键盘控制-->
    <node pkg="turtlesim" type = "turtle_teleop_key" name ="teleop_key" output = "screen"/>
    <!-- 生成新的一只乌龟-->
    <node pkg="tf2_turtle" type = "spawn_new_turtle" name ="turtle2" output = "screen"/>
    <!-- 启动两个乌龟相对世界的坐标关系的发布-->
    <node pkg="tf2_turtle" type = "tf2_pub_turtle" name ="pub1" args="turtle1" output = "screen"/>
    <node pkg="tf2_turtle" type = "tf2_pub_turtle" name ="pub2" args="turtle2" output = "screen"/>
    <!-- 订阅turtle1和turtle2相对于世界坐标系的坐标消息,并转换成turtle1相对于turtle2的坐标系,再生成速度消息-->
    <node pkg="tf2_turtle" type = "tf2_sub_turtle" name ="sub"  output = "screen"/>
</launch>

python版本的乌龟跟随实验与C++基本一致,具体可见TF坐标变换实操