本片博客 讲解 如何添加 一个 固定的坐标系 到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 轨迹如下
