文章目录
简介
所谓TF(TransForm)， 就是坐标转换，坐标变换包括了位置和姿态两个方面的变换， ROS中的tf是一个可以让用户随时记录多个坐标系的软件包。 tf保持缓存的树形结构中的坐标系之间的关系， 并且允许用户在任何期望的时间点在任何两个坐标系之间转换点， 矢量等。
tf的可以被当做是一种标准规范， 这套标准定义了坐标转换的数据格式和数据结构， tf本质是树状的数据结构，tf也可以看成是一个topic: /tf ， 话题中的message保存的就是tf tree的数据结构格式． 维护了整个机器人的甚至是地图的坐标转换关系。
观察上图，可以看到ROS数据结构的一个抽象图， ROS中机器人模型包含大量的部件，这些部件统称之为link,每一个link上面对应着一个frame, 即一个坐标系， link和frame概念是绑定在一起的，像上图可以看到又很多的frame,错综复杂的铺置在机器人的各个link上， 维护各个坐标系之间的关系， 就要靠着tf tree来处理。
每两个frame之间都会有一个Node来发布消息来broadcaster，来发布消息维系坐标转换，进而维护着各个坐标系之间的联通。
TF消息
TransformStamped.msg的格式规范如下：
std_mags/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/Transform transform
geometry_msgs/Vector3 translation
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation
float64 x
float64 y
flaot64 z
float64 w
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
许多的TransformStamped.msg发向tf， 形成了TF树
TF树的数据类型
TF tree是由很多的frame之间TF拼接而成
- tf/tfMessage.msg
- tf2_msgs/TFMessage.msg
TF的数据类型有两个， 主要的原因是版本的迭代。 自ROS Hydro以来， tf第一代已被“弃
用”， 转而支持tf2。 tf2相比tf更加简单高效
使用命令 rostopic info /tf 查看tf版本
格式定义
tf/tfMessage.msg或tf2_msgs/TFMessage标准格式规范如下:
geometry_msgs/TransformStamped[] transforms
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/Transform transform
geometry_msgs/Vector3 translation
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation
float64 x
float64 y
flaot64 z
float64 w
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
一个TransformStamped数组就是一个TF tree。
tf in c++
TF不仅仅是一个标准、 话题， 它还是一个接口。
数据类型
C++中给我们提供了很多TF的数据类型,如下：
tf::StampedTransform只能用在C++里， 只是C++的一个类， 一种数据格式， 并不是一个消息。而geometry_msgs/TransformStamped.msg是一个message,它依赖于ROS， 与语言无关，
数据转换
TF里有可能会遇到各种各样数据的转换， 常见的四元数、 旋转矩阵、 欧拉角这三种数据之间的转换
定义空间点和空间向量
//计算两个向量的点积
tfScalar::tfDot(const Vector3 &v1, const Vector3 &v2)
//计算向量的模
tfScalar length()
//求与已知向量同方向的单位向量
Vector3 &normalize()
//计算两个向量的夹角
tfScalar::tfAngle(const Vector3 &v1, const Vector3 &v2)
//计算两个向量的距离
tfScale::tfDistance(const Vector3 &v1, const Vector3 &v2)
//计算两个向量的乘积
tfScale::tfCross(const Vector3 &v1,const Vector3 &v2)
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
定义四元数
//由欧拉角计算四元数
setRPY(const tfScalar& yaw, const stScalar &pitch, const tfScalar &roll)
//由四元数得到旋转轴
Vector3 getAxis()
//已知旋转轴和旋转角估计四元数
setRotation(const Vector3 &axis, const tfScalar& angle)
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
定义旋转矩阵
//通过四元数得到旋转矩阵
setRotaion(const Quaternion &q)
//由旋转矩阵求欧拉角
getEulerYPR(tfScalar &yaw, tfScalar &pitch, tfScalar &roll )
- 1
- 2
- 3
- 4
- 5
TF类
tf::TransformBroadcaster类
transformBroadcaster()
void sendTransform(const StampedTransform &transform)
void sendTransform(const std::vector<StampedTransform> &transforms)
void sendTransform(const geometry_msgs::TransformStamped &transform)
void sendTransform(const std::vector<geometry_msgs::TransformStamped> &transforms)
- 1
- 2
- 3
- 4
- 5
broadcaster就是一个publisher,而sendTransform的作用是来封装publish的函数。 在实际使用中， 需要在某个Node中构建tf::TransformBroadcaster类， 然后调用sendTransform(),将transform发布到 /tf 的一段transform上
tf::TransformListener类
从 /tf 上接收的类
//lookupTransform得到从源坐标系到目标坐标系之间的转换关系
void tf::TransformListener::lookupTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
//target_frame：目标坐标系
//source_frame：源坐标系
//time：查询时刻，常用 ros::Time（0） ,这个表示为最新的坐标转换关系，而 ros::time::now 则会因为收发延迟的原因， 而不能正确获取当前最新的坐标转换关系
//transform：存储转换关系的位置
//canTransform()是用来判断两个transform之间是否连通
bool canTransform()
//waitForTransform()const是用来等待某两个transform之间的连通
bool waitForTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
//target_frame：目标坐标系
//source_frame：源坐标系
//time：查询时刻
//polling_sleep_duration 转换fail重试时间
//error_msg：保存转换失败的信息，转换成功为NULL
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
代码演示
C++代码的思路分为两个部分：
- 编写一个节点，广播两个参考系之间的tf变换关系
- 编写另外一个节点，订阅tf树，然后从tf树中遍历到两个参考系之间的变换公式，然后通过公式计算数据的变换。
tf.broadcaster.cpp
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
//退出用： ctrl+z
int main(int argc, char** argv){
//初始化
ros::init(argc, argv, "tf_broadcaster");
ros::NodeHandle node;
static tf::TransformBroadcaster br;
tf::Transform transform;
//geometry_msgs::Quaternion qw;
tf::Quaternion q;
//定义初始坐标和角度
double roll=0,pitch=0,yaw=0,x=1.0,y=2.0,z=3.0;
ros::Rate rate(1);
while(ros::ok())
{
yaw+=0.1;//每经过一秒开始一次变换
//输入欧拉角， 转化成四元数在终端输出
q.setRPY(roll,pitch,yaw);
//qw=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);方法2
transform.setOrigin(tf::Vector3(x,y,z));
transform.setRotation(q);
std::cout<<"发布tf变换： sendTransform函数"<<std::endl;
br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"base_link","link1"));
std::cout<<"输出的四元数为： w="<<q[3]<<",x="<<q[0]<<",y="<<q[1]<<",z="<<q[2]<<std::endl;
// std::cout<<"输出的四元数为： w="<<qw.w<<",x="<<qw.x<<",y="<<qw.y<<",z="<<qw.z<<std::endl;
rate.sleep();
ros::spinOnce();
}
return 0;
};
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
tf_listerner.cpp
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
int main(int argc, char argv){
ros::init(argc, argv, “tf_listener”);
ros::NodeHandle node;
tf::TransformListener listener;
//1. 阻塞直到frame相通
std::cout<<“1. 阻塞直到frame相通”<<std::endl;
listener.waitForTransform(“/base_link”,“link1”,ros::Time(0),ros::Duration(4.0));
ros::Rate rate(1);
while (node.ok()){
tf::StampedTransform transform;
try{
//2. 监听对应的tf,返回平移和旋转
std::cout<<“2. 监听对应的tf,返回平移和旋转”<<std::endl;
listener.lookupTransform(“/base_link”, “/link1”,ros::Time(0), transform);
//ros::Time(0)表示最近的一帧坐标变换， 不能写成ros::Time::now()
}
catch (tf::TransformException &ex)
{
ROS_ERROR(“%s”,ex.what());
ros::Duration(1.0).sleep();
continue;
}
std<span class="token double-colon punctuation">::</span>cout<span class="token operator"><<</span><span class="token string">"输出的位置坐标： x="</span><span class="token operator"><<</span>transform<span class="token punctuation">.</span><span class="token function">getOrigin</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">.</span><span class="token function">x</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token operator"><<</span><span class="token string">",y="</span><span class="token operator"><<</span>transform<span class="token punctuation">.</span><span class="token function">getOrigin</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">.</span><span class="token function">y</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token operator"><<</span><span class="token string">",z="</span><span class="token operator"><<</span>transform<span class="token punctuation">.</span><span class="token function">getOrigin</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">.</span><span class="token function">z</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token operator"><<</span>std<span class="token double-colon punctuation">::</span>endl<span class="token punctuation">;</span>
std<span class="token double-colon punctuation">::</span>cout<span class="token operator"><<</span><span class="token string">"输出的旋转四元数： w="</span><span class="token operator"><<</span>transform<span class="token punctuation">.</span><span class="token function">getRotation</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">.</span><span class="token function">getW</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token operator"><<</span><span class="token string">",x="</span><span class="token operator"><<</span>transform<span class="token punctuation">.</span><span class="token function">getRotation</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">.</span><span class="token function">getX</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token operator"><<</span><span class="token string">",y="</span><span class="token operator"><<</span>transform<span class="token punctuation">.</span><span class="token function">getRotation</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">.</span><span class="token function">getY</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token operator"><<</span><span class="token string">",z="</span><span class="token operator"><<</span>transform<span class="token punctuation">.</span><span class="token function">getRotation</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">.</span><span class="token function">getZ</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token operator"><<</span>std<span class="token double-colon punctuation">::</span>endl<span class="token punctuation">;</span>
rate<span class="token punctuation">.</span><span class="token function">sleep</span><span class="token punctuation">(</span><span class="token punctuation">)</span><span class="token punctuation">;</span>
<span class="token punctuation">}</span>
<span class="token keyword">return</span> <span class="token number">0</span><span class="token punctuation">;</span>
};
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
1028
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
