Chapter5 ROS常用组件(Ⅰ)----ROS坐标变换

目录

一、本章概述

二、TF坐标变换

2.1 本节引言

2.2 坐标msg消息

2.2.1 本节简介

2.2.2 静态坐标变换

2.2.3 C++实现静态坐标变换

2.2.3 python实现静态坐标变换 

2.2.4 补充知识点以及rviz的简单使用

2.3 动态坐标变换

2.3.1 引言

 2.3.2 C++实现动态坐标变换

2.3.3 python实现动态坐标变换 

2.4 多坐标变换

2.4.1 引言

2.4.2 C++实现多坐标变换

2.4.3 python实现多坐标变换

2.5 坐标系关系查看 

2.6 TF坐标变换案例实现

2.6.1 本节序言 

2.6.2 该案例的C++实现

2.6.3 该案例的python实现

2.7 本节总结

---

一、本章概述

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

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

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

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

二、TF坐标变换

2.1 本节引言

1.应用场景

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

场景1:雷达与小车

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

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

根据我们高中学习的知识，在明确了不同坐标系之间的的相对关系，就可以实现任何坐标点在不同坐标系之间的转换，但是该计算实现是较为常用的，且算法也有点复杂，因此在 ROS 中直接封装了相关的模块: 坐标变换(TF） 

2.基本概念

Ⅰ.TF ：TransForm Frame,坐标变换

Ⅱ.坐标系:ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。

2.2 坐标msg消息

2.2.1 本节简介

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

前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。

2.2.2 静态坐标变换

1.定义：

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

2.需求分析

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

3.实现分析

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

2.2.3 C++实现静态坐标变换

1.发布方设计

Ⅰ.复习从头实现  新建文件夹demo04_WS ,并在里面建立 src文件夹

mkdir demo04_ws
cd demo04_ws/
mkdir src

Ⅱ.进入工作空间，执行

catkin_make

Ⅲ.修改编译环境 按住ctrl + shift + B 粘贴下列代码

{
"version":"2.0.0",
"tasks":[
{
"label":"catkin_make:debug",
"type":"shell",
"command":"catkin_make",
"args":[],
"group":{"kind":"build","isDefault":true},
"presentation":{"reveal":"always"},
"problemMatcher":"$msCompile"
}]
}

Ⅳ.创建功能包 tf01_static ，导入依赖 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs

Ⅴ.创建C++文件 demo01_static_pub.cpp并完成相关配置

Ⅵ.逻辑流程

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

Ⅶ.code

#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"                             //发布对象的建立需要这个
#include "geometry_msgs/TransformStamped.h"                               //组织被发布消息需要这个
#include "tf2/LinearMath/Quaternion.h"
int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_pub");
    ros::NodeHandle nh;

    //发布对象
    tf2_ros::StaticTransformBroadcaster pub;
    //组织被发布消息
    geometry_msgs::TransformStamped tfs;
    tfs.header.stamp = ros::Time::now();
    tfs.header.frame_id = "base_link";                       //相对坐标系中被参考的哪一个，我们这里有雷达和小车主体，我们参考的是小车主体
    tfs.child_frame_id = "laser";                                   //雷达坐标系
    //设置雷达相对于base_link的相对量   也即雷达相对于小车主体的偏移量
    tfs.transform.translation.x = 0.2;
    tfs.transform.translation.y = 0.0;
    tfs.transform.translation.z = 0.5;
    //需要根据欧拉角转换

    tf2::Quaternion qtn;                    //创建四元数对象
    //向该对象设置欧拉角，这个对象可以将欧拉角转换成四元数
    qtn.setRPY(0.0,0.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();

    //发布数据
    pub.sendTransform(tfs);
    ros::spin();
    return 0;
}

Ⅷ.执行，被挂起

Ⅸ.查看rostopiclist  /tf_static 此话题出现

Ⅹ.用rostppic echo 打印该话题下发布的数据

liuhongwei@liuhongwei-virtual-machine:~$ rostopic echo /tf_static 
transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1650073520
        nsecs: 679891682
      frame_id: "base_link"
    child_frame_id: "laser"
    transform: 
      translation: 
        x: 0.2
        y: 0.0
        z: 0.5
      rotation: 
        x: 0.0
        y: 0.0
        z: 0.0
        w: 1.0

成功！

PS:另外可以通过图形化的方式查看两者关系

调用rviz命令行

2.订阅方设计 

Ⅰ.code：

#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[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;

    //创建一个 buffer 缓存
    //在创建一个监听对象，会把监听到的数据存入buffer中
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    //组织一个坐标点关系
    geometry_msgs::PointStamped ps;
    //障碍物是相对于雷达而言的，是相对于雷达的坐标系
    ps.header.frame_id = "laser";
    ps.header.stamp = ros::Time::now();
    ps.point.x = 2.0;
    ps.point.y = 3.0;
    ps.point.z = 5.0;

    //转换算法 需要调用tf内置实现
    ros::Rate rate(10);
    while(ros::ok())
    {
        //核心实现，将相对于雷达的坐标系转换为相对于base_link的坐标系
        geometry_msgs::PointStamped ps_out;
        ps_out =  buffer.transform(ps,"base_link");     //参数  被转换的坐标点，相对于哪个坐标系

        //输出
        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());

        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

Ⅱ.运行 

2.2.3 python实现静态坐标变换 

1.实现细节

Ⅰ.code

#! /usr/bin/env python
# encoding:utf-8

import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
"""
发布方实现：发布两个坐标系相对关系（小车底盘 ---baselink  雷达 --- laser）
流程：
1.导包
2.初始化ros结点
3.创建发布对象
4.组织被发布的数据
5.发布数据
6.spin
"""



if __name__ == "__main__":
    rospy.init_node("static_pub_p")

    pub = tf2_ros.StaticTransformBroadcaster()

    ts = TransformStamped()

    ts.header.seq = 100
    ts.header.stamp = rospy.Time.now()
    #:父系坐标系
    ts.header.frame_id = "base_link"
    ts.child_frame_id = "laser"
    ts.transform.translation.x = 2.0
    ts.transform.translation.y = 0.0
    ts.transform.translation.z = 0.5

    qtn = tf.transformations.quaternion_from_euler(0,0,0)
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]

    pub.sendTransform(ts)
    rospy.spin()
    pass

#! /usr/bin/env python
# encoding:utf-8

from ast import Try
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
"""
订阅方实现：订阅坐标变换信息，传入被转换的坐标点，调用转换算法
1.导包
2.初始化
3.创建订阅对象
4.组织被转换的坐标点
5.转化逻辑实现
6.输出最终的转换结果
7.spin()
"""

if __name__ == "__main__":

    rospy.init_node("static_sub_p")

    buffer = tf2_ros.Buffer()
    sub = tf2_ros.transform_listener(buffer)

    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

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            ps_out  = buffer.transform(ps,"base_link")
            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()
        
    pass

2.2.4 补充知识点以及rviz的简单使用

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

2.rviz

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

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

2.3 动态坐标变换

2.3.1 引言

1.定义

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

2.本节案例实现

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

3.实现分析

1. 乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点

2. 订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度

3. 将 pose 信息转换成 坐标系相对信息并发布

 2.3.2 C++实现动态坐标变换

1.发布方实现

Ⅰ.创建功能包 依赖于 tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs turtlesim 

Ⅱ.准备工作

需要订阅乌龟的位置信息，转换成相对于窗体的坐标关系并发布
准备信息： 话题

rostopic list

liuhongwei@liuhongwei-virtual-machine:~$ rostopic list
/rosout
/rosout_agg
/turtle1/cmd_vel
/turtle1/color_sensor
/turtle1/pose

查看位置话题发布的消息格式：rostopic info

liuhongwei@liuhongwei-virtual-machine:~$ rostopic info /turtle1/pose 
Type: turtlesim/Pose

Publishers: 
 * /turtlesim (http://liuhongwei-virtual-machine:42749/)

Subscribers: None

查看具体格式：

liuhongwei@liuhongwei-virtual-machine:~$ rosmsg info turtlesim/Pose 
float32 x
float32 y
float32 theta
float32 linear_velocity
float32 angular_velocity

Ⅲ.代码实现

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
发布方实现：需要订阅乌龟的位置信息，转换成相对于窗体的坐标关系并发布
准备信息： 话题 信息
流程：
1.包含头文件
2.设置编码、初始化、nodehandle
3.创建订阅对象、订阅 /turtle1/Pose
4.会调函数处理订阅的信息：需要将位置信息转换成坐标相对关系并发布
5.spin()
*/

void doPose(const turtlesim::Pose::ConstPtr & pose)
{
    //获取位置信息转换成坐标系相对关系并发布
    //1.创建发布对象
    static tf2_ros::TransformBroadcaster pub;

    //2.组织被发布的数据::乌龟相对于世界坐标系
    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;    //pose的x属性相当于乌龟坐标系相当于世界坐标系的坐标
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    //坐标系四元数的设置,位置信息中没有四元数，但是有个篇航角度，又乌龟是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();

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

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;

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

    //5.spin
    ros::spin();
    return 0;
}

Ⅳ.测试① 

Ⅴ.测试②

2.订阅方实现 

#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[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"static_sub");
    ros::NodeHandle nh;

    //创建一个 buffer 缓存
    //在创建一个监听对象，会把监听到的数据存入buffer中
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listener(buffer);

    //组织一个坐标点关系，这个坐标点是要被转换的坐标点
    geometry_msgs::PointStamped ps;
    //参考的坐标系：frame_id
    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;
    ros::Duration(2).sleep();
    //转换算法 需要调用tf内置实现
    ros::Rate rate(10);
    while(ros::ok())
    {
        //核心实现，将相对于雷达的坐标系转换为相对于base_link的坐标系
        geometry_msgs::PointStamped ps_out;
        ps_out =  buffer.transform(ps,"world");     //参数  被转换的坐标点，相对于哪个坐标系

        //输出
        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());

        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

2.3.3 python实现动态坐标变换 

 Ⅰ.发布方实现

#! /usr/bin/env python
#encoding:utf-8

"""
发布方实现：订阅乌龟的位置信息，转换成坐标系的相对关系，再发布
1.导包
2.初始化ros结点
3.创建订阅对象 
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.创建发布坐标系相对关系的对象
    pub = tf2_ros.TransformBroadcaster()
    #2.将pose位置信息转换成坐标系相对关系信息
    ts = TransformStamped()
    #被参考的坐标系
    ts.header.frame_id = "world"
    ts.child_frame_id = "turtle1"
    ts.header.stamp = rospy.Time.now()
    #子集坐标系相对于父集坐标系的偏移量
    ts.transform.translation.x = pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0
    #四元数的设置
    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)
    pass

if __name__ == "__main__":
    rospy.init_node("dynamic_pub_p")
    #创建订阅对象 : 话题名称 数据类型 回调函数
    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=100)

    rospy.spin()
    pass

Ⅱ.订阅方实现

#! /usr/bin/env python
# encoding:utf-8

from ast import Try
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
"""
订阅方实现：订阅坐标变换信息，传入被转换的坐标点，调用转换算法
1.导包
2.初始化
3.创建订阅对象
4.组织被转换的坐标点
5.转化逻辑实现
6.输出最终的转换结果
7.spin()
"""

if __name__ == "__main__":

    rospy.init_node("static_sub_p")

    buffer = tf2_ros.Buffer()
    sub = tf2_ros.transform_listener(buffer)

    ps = tf2_geometry_msgs.PointStamped()
    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

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            ps_out  = buffer.transform(ps,"world")
            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()
        
    pass

2.4 多坐标变换

2.4.1 引言

1.需求描述

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

2.实现分析

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

2.4.2 C++实现多坐标变换

1.编写launch文件

Ⅱ.rviz查看实现

Ⅲ.订阅方实现

#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"
/*
订阅方实现：
计算son1与son2的相对关系
计算son1中的某个点在son2中的坐标值
1.包含头文件
2.编码、初始化、nodehandle的实现
3.创建订阅对象
4.编写解析逻辑
5.spinonce
*/

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;
   
    //创建订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    ros::Rate rate(10);
    //创建坐标点，坐标点处于son1中
    geometry_msgs::PointStamped psSon1;
    psSon1.header.stamp = ros::Time::now();
    psSon1.header.frame_id = "son1";
    psSon1.point.x = 1.0;
    psSon1.point.y = 2.0;
    psSon1.point.z = 3.0;

    
    while(ros::ok())
    {
        //核心操作
        try
        {
            //1.计算son1与son2之间的相互关系(目标坐标系（son2），原坐标系（son1），时间：最近的取值) 
            //通过son1和son2相当于world的关系，能计算出son1到son2的相对关系，返回值是坐标系的相对关系
            geometry_msgs::TransformStamped son1toson2 =  buffer.lookupTransform("son2","son1",ros::Time::now());
            ROS_INFO("son1相对于son2的相对信息：父级名称：%s，子级名称：%s,偏移量（%.2f，%.2f，%.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 psSon2 =  buffer.transform(psSon1,"son2");   //参数一：被转换的坐标点  参数二：目标坐标系
            ROS_INFO("坐标点在son2中的值（%.2f，%.2f，%.2f）",psSon2.point.x,psSon2.point.y,psSon2.point.z);
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示：%s",e.what());
        }
        
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

Ⅳ.执行

PS：lookuotransform的参数说明

比如A相对于B的坐标系关系

参数一：目标坐标系 B

参数二：原坐标系 A

参数三：ros：：Time(0) 取间隔最短的两个坐标关系帧计算相对关系

返回值：geometry_msgs::TransformStamped 源相对于目标坐标系的相对关系

2.4.3 python实现多坐标变换

#! /usr/bin/env python
# encoding:utf-8

import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped
"""
订阅方实现：订阅坐标变换信息，传入被转换的坐标点，调用转换算法
1.导包
2.初始化
3.创建订阅对象
4.组织被转换的坐标点
5.转化逻辑实现
6.输出最终的转换结果
7.spin()
"""

if __name__ == "__main__":

    rospy.init_node("static_sub_p")

    buffer = tf2_ros.Buffer()
    sub = tf2_ros.TransformListener(buffer)

    ps = tf2_geometry_msgs.PointStamped()
    ps.header.stamp = rospy.Time.now()
    #参考的坐标系是乌龟坐标系
    ps.header.frame_id = "son1"
    ps.point.x = 1.0
    ps.point.y = 2.0
    ps.point.z = 3.0

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            #计算son1相对于son2之间的关系(目标坐标系，源坐标系，time)
            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")
            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()
        
    pass

2.5 坐标系关系查看 

本节主要介绍tf2_tools的相关使用

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

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

sudo apt install ros-noetic-tf2-tools

Ⅱ.使用

生成 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 文件

6.2.2查看文件

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

内如如图所示:

2.6 TF坐标变换案例实现

2.6.1 本节序言 

1.本节需求要求

程序启动之初: 产生两只乌龟，中间的乌龟(A) 和 左下乌龟(B), B 会自动运行至A的位置，并且键盘控制时，只是控制 A 的运动，但是 B 可以跟随 A 运行

 2.实现分析

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

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

经典比喻：
老美打老苏：

1.老美要设计一张世界地图

 2.老美导弹有自己的坐标，老苏的防御设施有自己的坐标

3.根据各自在世界地图的坐标实现坐标变换，计算出老苏的防御设施相对于老美导弹的相对坐标

2.6.2 该案例的C++实现

Ⅰ.新建功能包，添加依赖：

roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim

Ⅱ.乌龟窗体中新加一个小乌龟，利用launch文件

#include "ros/ros.h"
#include "turtlesim/Spawn.h"
/*
需求：向服务端发送请求，生成一只新乌龟
话题：/spawn
消息：turtlesim/Spawn
*/

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"service_call");
    ros::NodeHandle nh;
    ros::ServiceClient client = nh.serviceClient("/spawn");
    
    turtlesim::Spawn spawn;
    spawn.request.name = "turtle2";
    spawn.request.theta = 1.57;
    spawn.request.x = 1.0;
    spawn.request.y = 4.0;

    //ros::service::waitForService("/spawn");
    client.waitForExistence();

    bool flag = client.call(spawn);
    if(flag)
    {
        ROS_INFO("乌龟生成成功，新乌龟叫：%s",spawn.response.name.c_str());
    }
    else
    {
        ROS_INFO("请求失败");
    }
    return 0;
}

Ⅲ.执行

roslaunch tf04_test test.launch

 Ⅳ.如何控制第二只乌龟运动呢？？为什么是第一只乌龟在运动

我们查看rostopic list：发现和速度相关的话题有两个 

liuhongwei@liuhongwei-virtual-machine:~/demo04_ws$ rostopic list
/turtle1/cmd_vel
/turtle2/cmd_vel

而当前的键盘控制是turtle1订阅的，所以第二个乌龟订阅不到

调用命令 

liuhongwei@liuhongwei-virtual-machine:~/demo04_ws$ rostopic pub -r 10 /turtle2/cmd_vel geometry_msgs/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0" 

Ⅴ.发布方（发布两个乌龟的坐标信息）

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
发布方实现：需要订阅乌龟的位置信息，转换成相对于窗体的坐标关系并发布
准备信息： 话题 信息
流程：
1.包含头文件
2.设置编码、初始化、nodehandle
3.创建订阅对象、订阅 /turtle1/Pose
4.会调函数处理订阅的信息：需要将位置信息转换成坐标相对关系并发布
5.spin()
*/

std::string turtle_name;

void doPose(const turtlesim::Pose::ConstPtr & pose)
{
    //获取位置信息转换成坐标系相对关系并发布
    //1.创建发布对象
    static tf2_ros::TransformBroadcaster pub;

    //2.组织被发布的数据::乌龟相对于世界坐标系
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.header.stamp = ros::Time::now();
    ts.child_frame_id = turtle_name;
    //坐标系偏移量的设置
    ts.transform.translation.x = pose->x;    //pose的x属性相当于乌龟坐标系相当于世界坐标系的坐标
    ts.transform.translation.y = pose->y;
    ts.transform.translation.z = 0;
    //坐标系四元数的设置,位置信息中没有四元数，但是有个篇航角度，又乌龟是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();

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

int main(int argc, char  *argv[])
{

    //解析launch文件通过argv传入的参数

    setlocale(LC_ALL,"");
    ros::init(argc,argv,"dynamic_pub");
    ros::NodeHandle nh;
    if(argc!=2)
    {
        ROS_ERROR("请传入两个参数");
    }
    else
    {
            turtle_name = argv[1];
    }
    //订阅话题名称：turtle1或者turtle2要是动态传入的
    ros::Subscriber sub = nh.subscribe( turtle_name +"/pose",100,doPose);

    
    ros::spin();
    return 0;
}

Ⅵ.运行

 Ⅶ.订阅方实现：订阅并解析坐标信息并生成速度信息

①思路：

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

②code：

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

/*
需求：1.换算出turtle相对于turtle2之间的关系
            2.根据距离计算角速度&线速度并发布
*/

int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"tfs_sub");
    ros::NodeHandle nh;
   
    //创建订阅对象
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener sub(buffer);

    //A创建发布对象
    ros::Publisher pub = nh.advertise("/turtle2/cmd_vel",100);
    

    ros::Rate rate(10);
    //创建坐标点，坐标点处于son1中
    geometry_msgs::PointStamped psSon1;
    psSon1.header.stamp = ros::Time::now();
    psSon1.header.frame_id = "son1";
    psSon1.point.x = 1.0;
    psSon1.point.y = 2.0;
    psSon1.point.z = 3.0;

    
    while(ros::ok())
    {
        //核心操作
        try
        {
            //1.计算son1与son2之间的相互关系(父亲坐标系（son2），原坐标系（son1），时间：最近的取值) 
            //通过son1和son2相当于world的关系，能计算出son1到son2的相对关系，返回值是坐标系的相对关系
            //参数选择 父坐标系：导弹（turtle2） 子 （turtle1）
            geometry_msgs::TransformStamped son1toson2 =  buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
            ROS_INFO("相对信息：父级名称：%s，子级名称：%s,偏移量（%.2f，%.2f，%.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);
           
            //B根据相对信息计算并组织速度
            geometry_msgs::Twist twist;
            twist.linear.x = 0.5 * sqrt(pow(son1toson2.transform.translation.x,2))+sqrt(pow(son1toson2.transform.translation.y,2));
            twist.angular.z = 4* atan2(son1toson2.transform.translation.y,son1toson2.transform.translation.x);
            //C发布
            pub.publish(twist);
           
        }
        catch(const std::exception& e)
        {
            ROS_INFO("错误提示：%s",e.what());
        }
        
        rate.sleep();
        ros::spinOnce();
    }
    return 0;
}

Ⅷ.执行

2.6.3 该案例的python实现

Ⅰ.launch 生成乌龟

Ⅱ.生成乌龟的python代码

#! /usr/bin/env python
#encoding:utf-8

import rospy
from turtlesim.srv import Spawn,SpawnRequest,SpawnResponse

if __name__ =="__main__":
    rospy.init_node("service_call_p")
    # 话题类型 消息类型
    client = rospy.ServiceProxy("/spawn",Spawn)
    # 生成乌龟的信息 朝向等
    request = SpawnRequest()
    request.x = 4.5
    request.y = 2.0
    request.theta = -3
    request.name = "turtle2"

    client.wait_for_service()
    response = client.call(request)
    rospy.loginfo("生成乌龟的名字为：%s",response.name)
    pass

Ⅲ.发布两只乌龟的坐标信息

#! /usr/bin/env python
#encoding:utf-8

"""
发布方实现：订阅乌龟的位置信息，转换成坐标系的相对关系，再发布
1.导包
2.初始化ros结点
3.创建订阅对象 
4.回调函数处理订阅到的消息
5.spin()
"""

import rospy
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
import sys

turtle_name = ""

def doPose(pose):
    #1.创建发布坐标系相对关系的对象
    pub = tf2_ros.TransformBroadcaster()
    #2.将pose位置信息转换成坐标系相对关系信息
    ts = TransformStamped()
    #被参考的坐标系
    ts.header.frame_id = "world"
    ts.child_frame_id = turtle_name
    ts.header.stamp = rospy.Time.now()
    #子集坐标系相对于父集坐标系的偏移量
    ts.transform.translation.x = pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0
    #四元数的设置
    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)
    pass

if __name__ == "__main__":
    rospy.init_node("dynamic_pub_p")
    #创建订阅对象 : 话题名称 数据类型 回调函数

    #解析传入的参数（现在传入几个参数？文件全路径+传入的参数+结点名称+日志文件路径）
    if len(sys.argv)!=4:
        rospy.loginfo("传入参数部队")
        sys.exit(1)
    else:
        turtle_name = sys.argv[1]
    sub = rospy.Subscriber(turtle_name + "/pose",Pose,doPose,queue_size=100)

    rospy.spin()
    pass

Ⅳ.订阅坐标信息，转换成 乌龟A（车）相对于乌龟B（导弹）的相对信息

#! /usr/bin/env python
# encoding:utf-8

import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped
"""
订阅方实现：订阅坐标变换信息，传入被转换的坐标点，调用转换算法
1.导包
2.初始化
3.创建订阅对象
4.组织被转换的坐标点
5.转化逻辑实现
6.输出最终的转换结果
7.spin()
"""

if __name__ == "__main__":

    rospy.init_node("static_sub_p")

    buffer = tf2_ros.Buffer()
    sub = tf2_ros.TransformListener(buffer)

    ps = tf2_geometry_msgs.PointStamped()
    ps.header.stamp = rospy.Time.now()
    #参考的坐标系是乌龟坐标系
    ps.header.frame_id = "son1"
    ps.point.x = 1.0
    ps.point.y = 2.0
    ps.point.z = 3.0

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            #计算son1相对于son2之间的关系(目标坐标系，源坐标系，time)
            ts = buffer.lookup_transform("turtle2","turtle1",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)
        except Exception as e:
            rospy.logwarn("%s",e)
        rate.sleep()
        
    pass

Ⅴ.设置跟随

#! /usr/bin/env python
# encoding:utf-8

from cmath import sqrt
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped,Twist
import math
"""
订阅方实现：订阅坐标变换信息，传入被转换的坐标点，调用转换算法
1.导包
2.初始化
3.创建订阅对象
4.组织被转换的坐标点
5.转化逻辑实现
6.输出最终的转换结果
7.spin()
"""

if __name__ == "__main__":
    #2.初始化
    rospy.init_node("static_sub_p")
    #3.创建订阅对象
    buffer = tf2_ros.Buffer()
    sub = tf2_ros.TransformListener(buffer)

    # 创建速度消息发布对象
    pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=100)


    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            #计算son1相对于son2之间的关系(目标坐标系，源坐标系，time)
            ts = buffer.lookup_transform("turtle2","turtle1",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)

            #组织twist消息
            twist = Twist()
            twist.linear.x = 0.5 * math.sqrt(math.pow(ts.transform.translation.x,2) + math.pow(ts.transform.translation.y,2))
            twist.angular.z = 4 * math.atan2(ts.transform.translation.y,ts.transform.translation.x)
            #发布twist消息
            pub.publish(twist)
        except Exception as e:
            rospy.logwarn("%s",e)
        rate.sleep()
        
    pass

2.7 本节总结

坐标变换在机器人系统中是一个极其重要的组成模块，在 ROS 中 TF2 组件是专门用于实现坐标变换的，TF2 实现具体内容又主要介绍了如下几部分:

1.静态坐标变换广播器，可以编码方式或调用内置功能包来实现(建议后者)，适用于相对固定的坐标系关系

2.动态坐标变换广播器，以编码的方式广播坐标系之间的相对关系，适用于易变的坐标系关系

3.坐标变换监听器，用于监听广播器广播的坐标系消息，可以实现不同坐标系之间或同一点在不同坐标系之间的变换

4.机器人系统中的坐标系关系是较为复杂的，还可以通过 tf2_tools 工具包来生成 ros 中的坐标系关系图

5.当前 TF2 已经替换了 TF，官网建议直接学习 TF2，并且 TF 与 TF2 的使用流程与实现 API 比较类似，只要有任意一方的使用经验，另一方也可以做到触类旁通