tf介绍
- 坐标转换(TransForm:位置和姿态)
- 坐标变换是空间实体的位置描述,是从一种坐标系统变换到另一种坐标系统的过程。通过建立两个坐标系统之间一一对应关系来实现
- 下图为机器人几个部件之间的坐标关系
tf概念
- tf是一个用户随时间跟踪多个坐标系的包,机器人不同部位和世界的坐标系以
tree structure
的形式存储起来,tf管理一系列的树状结构坐标系之间的关系 - 允许用户在各个坐标系中进行点、向量的变换
- 通俗的说tf可以帮助我们实时的在各个坐标系中进行坐标转换
TF树样例
tf认知
环境搭建,运行已下命令安装相关插件资源sudo apt-get install ros- kinetic -ros-tutorials ros- kinetic -geometry-tutorials ros- kinetic -rviz ros- kinetic -rosbash ros- kinetic -rqt-tf-tree
运行以下命令打开demo程序roslaunch turtle_tf turtle_tf_demo.launch
将当前运行命令的窗口作为激活窗口,用箭头按键控制小乌龟移动,另外一只小乌龟讲会跟着移动
- 这个demo用tf库建立了三个坐标系,世界坐标系,乌龟1坐标系,乌龟2坐标系
- 在乌龟移动的过程中,tf库创建了一个tf发布器,用于发布乌龟1的坐标系在世界坐标系中的实时位置
- 利用一个tf接收器,接收该位置
- 计算两个乌龟坐标系的差异,让乌龟2跟着乌龟1移动
view_frames
:该命令会以一个树状图的形式,给我们展现出当前ros中tf发布的所有坐标系
运行命令生成当前坐标系树状图:rosrun tf view_frames
查看pdf文件:evince frames.pdf
从上图中可以看到
- tf广播了三个坐标系:世界坐标,乌龟1坐标系,乌龟2坐标系
- world坐标是 turtle1和 turtle2的父级
rqt_tf_tree是一个可以实时可视化显示当前tf发布的坐标系图
输入命令:rosrun rqt_tf_tree rqt_tf_tree
tf_echo 可以报告任意两个坐标系之间的变换。
用法:rosrun tf tf_echo [reference_frame] [targe_frame]
输入命令:rosrun tf tf_echo turtle1 turtle2
rviz和tf:rviz作为一个可视化工具,可以用来观测turtle坐标系
输入命令:rosrun rviz rviz -d
rospack find turtle_tf/rviz/turtle_rviz.rviz
用键盘控制小乌龟移动,观测rviz中坐标系的变化
tf消息
tf/tfMessage.msg
tf2_msgs/TFMessage.msg
- 查询tf的类型:
rostopic info /tf
tf相关数据类型
- 向量:
tf::Vector3
- 点:
tf::Point
- 四元数:
tf::Quaternion
- 3x3矩阵(旋转矩阵):
tf::Matrix3x3
- 位姿:
tf::Pose
- 变换:
tf::Transform
- 带时间戳的以上类型:
tf::Stamped<T>
- 带时间戳的变换:
tf::StampedTransform
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)
tf::TransformListener
类
void lookupTransform(const std::string &target_frame,
const std::string &source_frame, const ros::Time &time,
StampedTransform &transform) const
bool canTransform()
bool waitForTransform() const
操作小记
写一个tf发布器
创建一个tf发布器,发布tf关系,父坐标系为word,子坐标系为turtle_name
创建一个功能包
catkin_create_pkg learning_tf tf roscpp rospy turtlesim
在src文件下新建一个turtle_tf_broadcaster.cpp写入以下代码
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg){
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_broadcaster");
if (argc != 2){
ROS_ERROR("need turtle name as argument");
return -1;
};
turtle_name = argv[1];
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
};
修改CMakeLists.txt文件
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
编译:catkin_make
正常情况下将在下图所示的目录下生成turtle_tf_broadcaster节点
在learning_tf下新建一个launch文件夹,并在其中新建一个start_demo.launch文件
<launch>
<!-- 打开Turtlesim Node,小乌龟-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 打开键盘控制程序 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- Axes -->
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/>
<!-- 以turtle1为命名打开了一个发布器 -->
<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
<!-- 以turtle2为命名打开了一个发布器 -->
<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />
</launch>
运行命令查看当前tf树
rosrun rqt_tf_tree rqt_tf_tree
思考:为什么我们运行了两个tf_learning节点分别命名为turtle1和turtle2,而只有turtle1的坐标系发布出来了?
提示:可以运行rqt_graph
查看节点,得出答案
写一个tf监听器
创建一个订阅者:订阅乌龟的位置
在src文件下新建一个turtle_tf_listener.cpp写入以下代码
// 头文件包含
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
// 调用服务生成第二只乌龟
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle =
node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
// 创建一个话题发布器,发布第二只乌龟运行的速度
ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
// 创建tf侦听器listener
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok()){
tf::StampedTransform transform;
try{
// 最多等待10秒,如果提前得到了坐标的转换信息,直接结束等待
listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(10.0));
// 从tf上侦听turtle2在turtle1坐标系中的位置,将该位置存储在transform中
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));
// 计算turtle2跑的速度和方向
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
修改CMakeLists.txt文件
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES} )
编译:catkin_make
在start_demo.launch中添加以下代码:
<node pkg="learning_tf" type="turtle_tf_listener" name="listener" />
运行
roslaunch learning_tf start_demo.launch
效果
一般一步实现了前面例子的乌龟跟随
运行 rqt_graph
分析是turtle1跟着turtle2跑还是turtle2跟着turtle1跑?
添加一个坐标系
为什么要添加坐标系?
- 对于许多问题,在它自身的坐标系中思考是非常容易的,比如在二维激光雷达自身的坐标系中,推算障碍物的方位是非常容易的
- tf允许你为系统中所有的传感器,部件定义自身坐标系,前面讲述的urdf建模时,就自动生成了tf树
- 但如果要处理系统外部的问题,如果能够随时添加一个坐标系的话,将会非常方便
- tf所建立的坐标系关系为树状结构,他不允许坐标系结构中存在闭环,也就是说一个坐标系只能有一个父级,但是可以有多个子级
- 目前,我们的tf树包含三个坐标系,world,turtle1,turtle2。turtle1和turtle2为world的子级
- 因此如果我们想添加新的坐标系到tf中的话,我们必须选择一个现存的坐标系作为父级
操作小记
在src/learning_tf/src
下创建文件frame_tf_broadcaster.cpp,写入以下代码:
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf_broadcaster");
ros::NodeHandle node;
tf::TransformBroadcaster br;
tf::Transform transform;
ros::Rate rate(10.0);
while (node.ok()){
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
// 子坐标系在父坐标系中的初始姿态
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
// 发布一个以turtle1为父级命名为carrot1的坐标系
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
rate.sleep();
}
return 0;
};
在CMakeList.txt中添加如下代码:
add_executable(frame_tf_broadcaster src/frame_tf_broadcaster.cpp)
target_link_libraries(frame_tf_broadcaster ${catkin_LIBRARIES})
编译:catkin_make
在start_demo.launch中添加如下代码:<node pkg="learning_tf" type="frame_tf_broadcaster" name="broadcaster_frame" />
运行 roslaunch learning_tf start_demo.launch
运行命令查看tf树:rosrun rqt_tf_tree rqt_tf_tree
此时,操作键盘控制乌龟移动,发现跟之前的行为并没有什么区别
在turtle_tf_listener.cpp文件中做以下修改:
try{
listener.waitForTransform("/turtle2", "/carrot1", ros::Time(0), ros::Duration(10.0));
listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0), transform);
}
重新编译运行,查看效果跟之前的区别
之前,我们创建了一个固定关系的坐标系,它相对于父级是不会变化的。现在创建一个动态的坐标系
修改frame_tf_broadcaster.cpp中对应的代码:
while (node.ok()){
transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
rate.sleep();
}
编译运行,观察现象
tf和时间
- 在前面的内容中我们学习了如何跟踪tf树上的坐标系
- tf树是随时间变化的,tf为tf树上的每个坐标系之间的转换都会缓存一定的时间
- 我们用
lookupTransform()
函数可获取到最近的转换关系,但是我们并不知道该转换关系对应的时间 - 接下来学习如何得到一个转换关系在一个特定的时间
操作小记
打开turtle_tf_listener.cpp,原来代码为:ros::Time(0)
参数指的就是获取当前最新的转化关系,将代码做如下修改:
该段代码是指获取当前时间点的转换关系
编译:catkin_make
运行:roslaunch learning_tf start_demo.launch
运行时会出现一个错误:该错误是由于turtle2的生成需要一定的时间,当我们第一次要询问转换关系时,turtle2还没有生成
如何获取5S前的坐标系转换关系?
修改代码如下:
编译:catkin_make
运行:roslaunch learning_tf start_demo.launch
感觉第二只小乌龟完全不受控!
原因在于,我们获取的相对关系是5s前的,5s前的turtle2和turtle1之间的关系
如何让turtle2跟着turtle1 5s前的位置运行呢?
一共包含了6个参数:
- 要转换的目标坐标系;
- 在什么时间;
- 源坐标系;
- 在什么时间;
- 不随时间变化的坐标系;
- 存储结果的变量
上图显示了坐标系转换的过程:
- tf计算在past的时间点turtle1和世界坐标系之间的关系
- 随着时间推移到达现在的时间点,计算turtle2和世界坐标系之间的关系
- tf计算当前turtle2和past时间点turtle1之间的关系
编译:catkin_make
运行:roslaunch learning_tf start_demo.launch
来源:CSDN
作者:duganlx
链接:https://blog.csdn.net/qq_40626497/article/details/103751901