本文为原创博客, 转载请注明出处:https://blog.csdn.net/q_z_r_s
机器感知 一个专注于SLAM、三维重建、机器视觉等相关技术文章分享的公众号 |
开源地址:点击该链接
tf入门教程
1. 引言
本节将告诉你tf
可以做什么, 将使用turtlesim
来展示它的威力. 接下来也将涉及到tf_echo
, view_frames
, rqt_tf_tree
和rviz
的使用.
首先安装必须要的packages
$ sudo apt-get install ros-indigo-ros-tutorials ros-indigo-geometry-tutorials ros-indigo-rviz ros-indigo-rosbash ros-indigo-rqt-tf-tree
运行demo
, 此程序的效果是实现跟随运动.
$ roslaunch turtle_tf turtle_tf_demo.launch
可通过view_frames
工具来创建一个坐标系示意图, 保存格式为pdf
文件
$ rosrun tf view_frames
#evince为Ubuntu系统自带的的文档阅读器
$ evince frames.pdf
另一个工具是时时运行的rqt_tf_tree
, 可以刷新状态
$ rosrun rqt_tf_tree rqt_tf_tree
#后台运行,不影响终端的使用,可以在弹出的窗口中选择需要运行的package
$ rqt &
#此次运行PID为5837, 可通过如下指令关闭此程序
$ kill 5837
tf_echo
实时broadcast
两个坐标系之间的转换关系
#Usage:
rosrun tf tf_echo [reference_frame] [target_frame]
$ rosrun tf tf_echo turtle1 turtle2
可视化工具rviz
, 它是一个测试tf frames
很有用的工具,
#第一次运行不知道为什么没起来, 提示`核心已转储`, 估计是内存不够用了,再次运行就可以了.
$ rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz
2. 写一个tf broadcaster
本节讲述如何把机器人的坐标系broadcast
给tf
, 接下来创建一个新的package
, 依赖tf
, roscpp
, rospy
, turtlesim
.
$ cd %YOUR_CATKIN_WORKSPACE_HOME%/src
$ catkin_create_pkg learning_tf tf roscpp rospy turtlesim
$ cd %YOUR_CATKIN_WORKSPACE_HOME%/
$ catkin_make
$ source ./devel/setup.bash
#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 包含`平移`和`旋转`
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
//RPY:Roll(x), Pitch(y), Yaw(z)
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
//真正将坐标转换传给`tf`的代码
//参数: 转换, 时间戳, 父坐标系(这里为世界坐标系), 子坐标系名字(这里为其本身)
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv){
//第三个参数:节点名, 在*.launch中可以重新定义, 如果rosrun则使用这里定义的名字.
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;
};
#添加编译
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
<!-- 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"/>
<!-- 包名 可执行程序名 参数 节点名 -->
<node pkg="learning_tf" type="turtle_tf_broadcaster"
args="/turtle1" name="turtle1_tf_broadcaster" />
<node pkg="learning_tf" type="turtle_tf_broadcaster"
args="/turtle2" name="turtle2_tf_broadcaster" />
</launch>
#终端实时显示两坐标系之间的变换, 原来此时turtle2还没有运行起来,
#害了我试了半天,看来看wiki要看全才行, 如何使用turtle2, 见后续的教程
#If you run tf_echo for the transform between the world and
#turtle 2, you should not see a transform, because the second turtle is not there yet. #However, as soon as we add the second turtle in the next tutorial, the pose of turtle 2
#will be broadcast to tf.
$ rosrun tf tf_echo /world /turtle1
3. 写一个listener
//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服务
turtlesim::Spawn srv;
//通过call函数调用服务, 并将结果存储在srv中, 返回bool, 正常时返回true
add_turtle.call(srv);
//Twist含有两部分: linear[x, y, z] angular[x, y, z]
//配置向topic:turtle2/cmd_vel发送message
ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok()){
tf::StampedTransform transform;
//获取turtle2, turtle1 之间的 transform(从1到2的变换)
try{
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));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
#添加listener编译
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
<!-- 追加启动listener节点 -->
<launch>
...
<node pkg="learning_tf" type="turtle_tf_listener"
name="listener" />
</launch>
4. 添加一个坐标系
tf
建立了一个坐标系树结构(tree structure)
, 它不允许在坐标系结构中出现闭环. 这意味着, 一个坐标系仅可以有一个父坐标系
, 但可以拥有多个子坐标系
. 当前, 我们的tf tree
有三个坐标系: world
, turtle1
, turtle2
. 两个turtles
是world
的子坐标系. 如果我们向tf
添加一个新的坐标系, 它们三个中就需要有一个被当做父坐标系, 且这个新的坐标系将变为子坐标系. 接下来添加一个新的坐标系carrot
, 父坐标系为turtle1
.
//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) );
//坐标系变换 父坐标系 子坐标系
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
rate.sleep();
}
return 0;
};
add_executable(frame_tf_broadcaster src/frame_tf_broadcaster.cpp)
target_link_libraries(frame_tf_broadcaster ${catkin_LIBRARIES})
<launch>
...
<node pkg="learning_tf" type="frame_tf_broadcaster"
name="broadcaster_frame" />
</launch>
以上程序只是添加了一个坐标系, 对原始程序没有影响, turtles
的运行还是原来的样子, 接下来进行修改:
//src/turtle_tf_listener.cpp
listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0), transform);
重新编译, 运行start_demo.launch
. 接下来跟踪一个动态的坐标系:
//src/frame_tf_broadcaster.cpp, sin-cos会导致画圆圈运动
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) );
5. 了解tf
和time
前边的教程我们学习了如何保持跟踪坐标系树, 这棵树随着时间改变, 默认情况下, tf
每10s
存储一次time snapshot
. 目前我们使用lookupTransform()
函数来获取tf树
中latest availabel transforms
, 却不知道这个变换是什么时候被记录的, 本节将讲述如何获取一个具体时刻的transform
. 修改代码如下:
//修改src/turtle_tf_listener.cpp如下代码
try{
listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0), transform);
//其中Time(0)中`0`表示`the latest available`
try{
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
//最后的代码, `now()`表示`the current time`
try{
listener.lookupTransform("/turtle2", "/turtle1", ros::Time::now(), transform);
以上代码运行之后, 会持续的提示错误:
[ERROR] [1287871653.885277559]: You requested a transform that is 0.018 miliseconds in the past, but the most recent transform in the tf buffer is 7.681 miliseconds old. When trying to transform between /turtle1 and /turtle2.
....
原因是: 每个listener
有一个buffer
来存储来自所有不同的tf boradcasters
的坐标系转换. 当broadcaster
发送一个transform
时, 这需要一些时间
才能将transform
传入到buffer
中(通常来说只有几个毫秒
). 因此, 当你请求一个在时间now
的坐标变换, 你需要等待a few milliseconds
来等待transform
的到来. 修改代码如下:
try{
ros::Time now = ros::Time::now();
//等待在now时刻的从turtle1到turtle1的变换, 持续等待时长为3.0
listener.waitForTransform("/turtle2", "/turtle1", now, ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1", now, transform);
不过, 这次在开始时刻仍然会看到一些错误:
[ERROR] [1287872014.408401177]: You requested a transform that is 3.009 seconds in the past, but the tf buffer only has a history of 2.688 seconds.
When trying to transform between /turtle1 and /turtle2.
这是因为, turtle2
的建立和开始publish
自己的tf frame
是需要时间的, 所以刚开始会出现没有找到需要的最新坐标变换. 等到所有的都就绪之后就一切正常了. 你会发现, 此时的跟踪效果和之前并没有什么明显的差别
, 这是因为实际的时间差只有几个毫秒而已, 所以并不会带来什么明显的改善. 之所以把Time(0)
改为now()
是想告诉大家有关tf buffer
和time delays
而已. 在真正的使用tf
时, 通常使用TIme(0)
就足够了.
6. 时间旅行(time travel)
本节将会进一步探讨tf
的基本概念和时间
. 讲述一个最有用的tf tricks
. 实现跟随5秒
之前的轨迹.
//方法一: 结果是小乌龟疯了, 乱跑起来了!
try{
ros::Time past = ros::Time::now() - ros::Duration(5.0);
listener.waitForTransform("/turtle2", "/turtle1", past, ros::Duration(1.0));
listener.lookupTransform("/turtle2", "/turtle1", past, transform);
//方法二: 完美实现!
try{
ros::Time now = ros::Time::now();
ros::Time past = now - ros::Duration(5.0);
listener.waitForTransform("/turtle2", now, "/turtle1", past, "/world",
ros::Duration(1.0));
listener.lookupTransform("/turtle2", now, "/turtle1", past, "/world", transform);