本片博客 讲解 如何添加 一个 固定的坐标系 到TF2

之前的博客 创建了 一个 乌龟的demo，添加了TF2 的 广播者和监听者。

这个和创建 TF2 的 广播者 很类似。

1、为什么添加 坐标系

对于很多任务在本地坐标系中处理很容易。例如激光雷达的数据在以雷达扫描器的中心为坐标系下最容易处理。
TF2 可以定义一个本地坐标系对每一个传感器、link等等。
tf2将处理所有引入的坐标系转换。

2、坐标系添加到了哪

TF2 对所有 的 坐标系 建立 了 一个 树行 结构 ， 在整体结构中 不 能 存在闭环。也就是说 一个坐标系 只有一个 父坐标系，可以有很多的子坐标系。

之前的两只乌龟的例子含有三个坐标系：世界坐标系、乌龟1坐标系、乌龟2坐标系。

两个乌龟的坐标系是世界坐标系的子坐标系。

如果我们想要添加一个新的坐标系到TF2，这三个存在的坐标系一个会成为父坐标系，新的坐标系即为子坐标系。

3、如何添加一个坐标系

下面的例子 会在 之前的例子的基础上 在 乌龟1 的坐标系 下 建立一个新 的 坐标系

3.1 代码

在learning_tf2 的功能包的src 文件夹下 创建一个新的cpp文件 命名为 frame_tf2_broadcaster.cpp.

#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf2_broadcaster");
  ros::NodeHandle node;

   tf2_ros::TransformBroadcaster tfb;
  geometry_msgs::TransformStamped transformStamped;

  
  transformStamped.header.frame_id = "turtle1";
  transformStamped.child_frame_id = "carrot1";
  transformStamped.transform.translation.x = 0.0;
  transformStamped.transform.translation.y = 2.0;
  transformStamped.transform.translation.z = 0.0;
  tf2::Quaternion q;
        q.setRPY(0, 0, 0);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

  ros::Rate rate(10.0);
  while (node.ok()){
    transformStamped.header.stamp = ros::Time::now();
    tfb.sendTransform(transformStamped);
    rate.sleep();
    printf("sending\n");
  }

};

3.2 代码解释

这个 代码 和 之前 写的例子 广播 乌龟 坐标系 的 代码 基本类似
只是 之前 是 把 世界坐标系 作为 父坐标系 乌龟坐标系为 子 坐标系

这个是把 乌龟1坐标系为 父 坐标系 ， 新坐标系 carrot1 为 子坐标系

   tf2_ros::TransformBroadcaster tfb;
  geometry_msgs::TransformStamped transformStamped;

  
  transformStamped.header.frame_id = "turtle1";
  transformStamped.child_frame_id = "carrot1";
  transformStamped.transform.translation.x = 0.0;
  transformStamped.transform.translation.y = 2.0;
  transformStamped.transform.translation.z = 0.0;
  tf2::Quaternion q;
        q.setRPY(0, 0, 0);
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

主要代码就是这部分 。 carrot1坐标系和turtle1坐标系 在y轴方向上偏移 2 m

4 、运行 坐标系 广播者 这个例子

在 CMakeLists.txt 中 加入 如下 代码：

add_executable(frame_tf2_broadcaster src/frame_tf2_broadcaster.cpp)
target_link_libraries(frame_tf2_broadcaster
 ${catkin_LIBRARIES}
)

编译成功的话

会 在工作空间 的这个路径下面 生成 frame_tf2_broadcaster 的 bin 文件

下面添加 launch 文件 给这个demo

5、测试结果

下面 更改 之前的 坐标系 监听者 的例子
让乌龟2 跟着 这个新的坐标系走

监听 坐标系变换的 地方 改成 下面的代码

  transformStamped = listener.lookupTransform("/turtle2", "/carrot1",
                           ros::Time(0));

再运行 launch 文件 乌龟2 不会 再去 乌龟1 那里 ，而是去了 乌龟 1 y轴上方 2m 的位置 。说明新的坐标系carrot1 成功 生成了

移动乌龟1 会 发现 乌龟2 的位置 和 乌龟1 的位置始终保持 左手边 2 m 的位置，因为 新的坐标系carrot1和乌龟1的坐标系的位置关系没有变化。

下面做一个 移动的坐标系

6、发布一个移动的坐标系

刚才添加的坐标系是一个固定的坐标系（和父坐标系关系不变）。

如果想发布一个 移动的坐标系 ，可以 随着时间变化更改 广播者。

下面修改 /carrot1 坐标系 和 /turtle1 坐标系的相对关系 随着时间变化。

在while循环中添加下面代码 ，在发送 坐标变换之前加

    transformStamped.transform.translation.x = 2.0*sin(ros::Time::now().toSec());
    transformStamped.transform.translation.y = 2.0*cos(ros::Time::now().toSec());

重新编译运行

再运行会发现乌龟2 不会 移动到一个地方不动了， 而是围者乌龟1 不停的转圈。

控制乌龟1 移动 乌龟2 轨迹如下

7、完毕