tf入门教程

本文为原创博客, 转载请注明出处: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);