ROS tf 编程指南

ROS (Robot Operating System, 机器人操作系统)是最知名的机器人操作系统，广泛应用于无人驾驶和机器人，tf（transforms，坐标系转换）是ROS下的一个常用的工具库。ros wiki 下给出了一个清晰的tutorial，然而，tf tutorial为了给出清晰的示例相对冗长，本文对tf tutorial的前六节进行了一个简略的总结，囊括了tf的发报、订阅和Debug等，希望对tf的使用有个精炼的介绍。

tf broadcaster

#include <tf/transform_broadcaster.h> //头文件
static tf::TransformBroadcaster br; //类定义
// world frame is parent frame, turtle_name frame is child frame 
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name)); //使用

tf listener

#include <tf/transform_listener.h> //头文件
tf::TransformListener listener; //类定义
// 使用
tf::StampedTransform transform;
try{
// ros::Time(0)表示最近可获得的transform
  listener.lookupTransform("/turtle2", "/turtle1",  
                           ros::Time(0), transform);
}
catch (tf::TransformException ex){
  ROS_ERROR("%s",ex.what());
  ros::Duration(1.0).sleep();
}

add a frame

tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
// ros::Time::now() 表示当前时间
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));

tf and time & time travel

• 获取最新的transform

// 使用ros::Time::now() 获取当前时间点的transform可能获取不到，因为获取transform有几毫米延时，当前时间点的transform还没有到达。
listener.lookupTransform("/turtle2", "/turtle1", ros::Time::now(), transform);
//改用以下两种方式
//第一种：
try{
    ros::Time now = ros::Time::now();
    // ros::Duration(3.0)表示最长等3.0秒
    listener.waitForTransform("/turtle2", "/turtle1",
                          now, ros::Duration(3.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                         now, transform);
    }
//第二种：
try{
// ros::Time(0)表示最近可获得的transform
  listener.lookupTransform("/turtle2", "/turtle1",  
                           ros::Time(0), transform);
}

• 获取过去的transform

try{
    // 获取5.0秒前的transform
    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);
}

debug tf

• 命令行

// 检查tf关系, 输出T_turtle3_turtle1, turtle3为base
rosrun tf tf_echo turtle3 turtle1
// 绘制tf tree
rosrun tf view frame
// 查看上述命令生成的frame.pdf文件
evince frame.pdf
// 监控tf,可以获得tf延时情况
rosrun tf tf_monitor turtle2 turtle1