本片博客讲解如何添加一个固定的坐标系到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 成功生成了

ROS TF2 添加一个坐标系附实例

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

ROS TF2 添加一个坐标系附实例

下面做一个移动的坐标系

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 不停的转圈。

ROS TF2 添加一个坐标系附实例