TF坐标变换

ROS小乌龟跟随

5.1 TF坐标变换 · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程

tf模块：在 ROS 中用于实现不同坐标系之间的点或向量的转换。

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

tf2_geometry_msgs:可以将ROS消息转换成tf2消息。

tf2: 封装了坐标变换的常用消息。

tf2_ros:为tf2提供了roscpp和rospy绑定，封装了坐标变换常用的API。

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

1.坐标msg消息

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

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

2.静态坐标变换

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

创建项目功能包依赖于tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs

注意子级与父级坐标系的关系以及坐标转换函数transform()使用的方法。

3.动态坐标变换

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

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

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

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

ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,callback);


void callback(const turtlesim::Pose::ConstPtr& pose){
    
    geometry_msgs::TransformStamped tfs;
    //  |----头设置
    tfs.header.frame_id = "world";
    tfs.header.stamp = ros::Time::now();

    //  |----坐标系 ID
    tfs.child_frame_id = "turtle1";

    //  |----坐标系相对信息设置
    tfs.transform.translation.x = pose->x;
    tfs.transform.translation.y = pose->y;
    tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 0
}

4.多坐标变换

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

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

使用静态坐标变换发布son1 son2 分别相对于world的坐标关系

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" />
</launch>

解析 son1 中的点相对于 son2 的坐标

geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));
ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str());
ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str());
ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f",
         tfs.transform.translation.x,
         tfs.transform.translation.y,
         fs.transform.translation.z
         );

然后解析坐标，求出该点在 son2 中的坐标。

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

geometry_msgs::PointStamped psAtSon2;
psAtSon2 = buffer.transform(ps,"son2");
ROS_INFO("在 Son2 中的坐标:x=%.2f,y=%.2f,z=%.2f",
         psAtSon2.point.x,
         psAtSon2.point.y,
         psAtSon2.point.z
         );

5.坐标系关系查看

输入下面命令查看是否包含该功能包

rospack find tf2_tools

如果没有，请使用如下命令安装:

sudo apt install ros-noetic-tf2-tools

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

rosrun tf2_tools view_frames.py

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

6.TF坐标变换实操

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

输入命令

rosrun turtlesim turtlesim_node

此时只出现了一只小乌龟，那么如何创建第二只小乌龟，并且不受键盘控制而是跟随第一只小乌龟运动呢？我们需要使用rosservice,话题使用的是 spawn.

创建出第二只小乌龟后，需要做的是订阅两只小乌龟的位姿信息并转换为坐标信息，之后获取第一只小乌龟相对于第二只小乌龟的坐标信息，这里要注意是turtle1相对于turtle2的坐标信息，不要搞反了，最后向第二只小乌龟发布相应的速度信息跟随第一只小乌龟。

5.1.6 TF坐标变换实操 · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程

7.TF2与TF

TF2已经替换了TF，TF2是TF的超集，建议学习 TF2 而非 TF。

TF2 功能包的增强了内聚性，TF 与 TF2 所依赖的功能包是不同的，TF 对应的是tf包，TF2 对应的是tf2和tf2_ros包，在 TF2 中不同类型的 API 实现做了分包处理。

TF2 实现效率更高，比如在:TF2 的静态坐标实现、TF2 坐标变换监听器中的 Buffer 实现等。

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

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

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

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

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