ROS学习笔记6 —— TF的使用

1. tf数据类型¹

在tf/transform_datatypes.h中定义，基本数据类型有：Quaternion, Vector, Point, Pose, Transform

• tf::Stamped 对上述数据类型做模板化（除了tf::Transform），并附带元素frame_id_和stamp_
• tf::StampedTransform 是tf::Transforms的特例，它要求frame_id 、stamp_(与此转换关联的时间戳) 、child_frame_id
• 辅助函数：
  • tf::Quaternion createIdentityQuaternion()，返回四元数句柄
  • tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw)，返回从固定轴的Roll, Pitch and Yaw(滚动，俯仰和偏转)构造的tf::Quaternion四元数
  • geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll,double pitch,double yaw)，返回从固定轴的Roll, Pitch and Yaw(滚动，俯仰和偏转)构造的geometry_msgs::Quaternion四元数

2. tf坐标变换²

Frame是坐标系统，在ROS里都是以3D形式存在，右手原则，X向前，Y向左，Z向上

Points在坐标系以tf::Point形式表达，等同与bullet类型的btVector3

在坐标系W里，坐标的点p，可以用：Wp

3. tf广播变换³

• 在节点里广播变换，推荐使用tf::TransformBroadcaster（广播器）.

• 发送变换通过调用sendTransform()函数实现，传递StampedTransform或geometry_msgs::TransformStamped为参数

void sendTransform(const StampedTransform & transform);
void sendTransform(const geometry_msgs::TransformStamped & transform);

4. 使用已发布的变换⁴

• 通过调用 tf::TransformListener类(监听器)来处理变换，它继承自tf::Transformer

核心方法：

• （1）构造函数

    TransformListener(const ros::NodeHandle &nh, ros::Duration max_cache_time=ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread=true)
    TransformListener(ros::Duration max_cache_time=ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread=true)
  

• （2）辅助方法

    std::string tf::TransformListener::resolve (const std::string &frame_id)
  

  结合tf_prefix获取解析的frame_id

• （3）canTransform()函数
  返回bool ，判断能否实现变换。不会抛出异常，如果出错，会返回error_msg的内容。

  • 基本API：

    bool tf::TransformListener::canTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const 
    // 检查在时间time，source_frame能否变换到target_frame
    

  • 高级API:

    bool tf::TransformListener::canTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, std::string *error_msg=NULL) const 
    // 检查在时间source_time，source_frame能否变换到fixed_frame，那么再实现在target_time变换到target_frame
    

• （4）waitForTransform() 函数
  返回bool值，评估变换是否有效

  • 基本API：

    bool tf::TransformListener::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 
    // 检查在时间time， source_frame能否变换到target_frame
    // 它将休眠并重试每个polling_duration，直到超时的持续时间已经过去。 它不会抛出异常。 在一个错误的情况下，如果你传递一个非NULL字符串指针，它会填充字符串error_msg。 （注意：这需要大量的资源来生成错误消息。）
    

  • 高级API：

    bool tf::TransformListener::waitForTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const 
    // 测试在source_time时间，source_frame能否变换到fixed_frame，那么在target_time，变换到target_frame。
    

• （5）lookupTransform() 函数

  • 返回两个坐标系的变换

  • 这个方法是 tf库的核心方法。大部分transform的方法都是终端用户使用，而这个方法设计在transform()方法内使用的

  • 返回的变换的方向：将source_frame变换到target_frame（注意：此处创客智造描述有误）

  • 这个方法会抛出TF的异常

  • 基本API：

    void tf::TransformListener::lookupTransform (const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const 
    // 在时间time上使用从source_frame到target_frame的变换填充transform
    

  • 高级API：

    void tf::TransformListener::lookupTransform (const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, StampedTransform &transform) const 
    // 在source_time使用从source_frame到fixed_frame的变换填充transform，在target_time从fixed_frame到target_frame的链接变换。
    

• transformDATA 方法
  tf::TransformListener类的主要目的是在坐标系间进行变换数据。

  • 基本API：

    void tf::TransformListener::transformDATATYPE (const std::string &target_frame, const geometry_msgs::DATATYPEStamped &stamped_in, geometry_msgs::DATATYPEStamped &stamped_out) const 
    // 将数据stamped_in转换为target_frame，使用标记的数据类型中的frame_id和stamp作为源。
    

  • 高级API：

    void tf::TransformListener::transformDATATYPE (const std::string &target_frame, const ros::Time &target_time, const geometry_msgs::DATATYPEStamped &pin, const std::string &fixed_frame, geometry_msgs::DATATYPEStamped &pout) const 
    // 将数据stamped_in转换为fixed_frame。 使用标记数据类型中的frame_id和stamp作为源。 然后在target_time从fixed_frame变换到target_frame。
    

  

5. 异常⁵

• roscpp所有的异常都继承基类ros::Exception

• ros::InvalidNodeNameException,当无效的基本名称传递到 ros::init()就会抛出异常，通常是带了/.

• ros::InvalidNameException,如果名称以~开始传递到任何的NodeHandle方法都会抛出异常;无效名传递到roscpp函数都会抛出异常。

• 所有在TF里异常继承自tf::TransformException，它继承自std::runtime_error。

• 异常类：tf::ConnectivityException
  作用：如果由于两个坐标系ID不在同一个连接的树中而无法完成请求，则抛出。

• 异常类：tf::ExtrapolationException
  作用：如果请求的坐标系id之间存在连接，但一个或多个变换已过期，则抛出。

• 异常类：tf::InvalidArgument
  作用：如果参数无效则抛出。 最常见的情况是非规范化的四元数。

• 异常类：tf::LookupException
  作用：如果引用了未发布的坐标系ID，则抛出。

6. 编写tf广播⁶

创建的功能包需要依赖于tf、roscpp、rospy，以小乌龟Demo为例，还需要添加依赖turtlesim

// tf包提供了TransformBroadcaster的实现，以帮助使发布变换的任务更容易
// 要使用TransformBroadcaster，我们需要包含tf/transform_broadcaster.h头文件

#include
#include
#include

using namespace std;
std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    // TF广播器
    // 创建一个TransformBroadcaster对象，我们稍后将使用它通过线路发送转换
    static tf::TransformBroadcaster br;

	// 创建一个Transform对象，并将信息从2D乌龟姿势复制到3D变换中
    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);

    // 发布坐标变换
    // 使用TransformBroadcaster发送转换需要四个参数；首先传递转换对象，采用ros::Time::now()来给发布的变换一个时间戳；然后需要传递创建的链接的父框架名称，此处为“world”；最后需要传递正在创建的链接的子框架名称，此处为乌龟本身名称
    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;
    
}

注意：sendTransform和StampedTransform具有父对象和子对象的相反顺序。

使用tf_echo工具检查位姿是否广播到tf:
rosrun tf tf_echo /world /turtle1

7. 编写tf监听器⁷

// tf包提供了TransformListener的实现，以帮助使接收变换的任务更容易
// 要使用TransformListener，我们需要包括tf/transform_listener.h头文件

#include 
#include 
#include 
#include 

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("spawn");
  turtlesim::Spawn srv;
  add_turtle.call(srv);

  ros::Publisher turtle_vel = node.advertise("turtle2/cmd_vel", 10);

  // 创建一个TransformListener对象,一旦监听器被创建，它开始接收tf转换，并缓冲它们长达10秒;TransformListener对象应该被限定为持久化，否则它的缓存将无法填充，并且几乎每个查询都将失败;一个常见的方法是使TransformListener对象成为一个类的成员变量
  
  tf::TransformListener listener;

  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{

      //  查询监听器进行特定的转换,从/turtle2坐标系开始变换到/turtle1坐标系,变换的时间，提供ros::Time(0)即会给出最近的可用的变换,结果存放的变换对象
      listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

	// 变换用于计算龟的新的线性和角速度，基于它与龟的距离和角度。
	// 新的速度发布在话题"turtle2/cmd_vel"中，turtlesim将使用它来更新turtle2的运动。
	
    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;
};

8. 增加坐标系⁸

• tf建立坐标系的树结构; 它不允许在坐标系结构中存在闭环
• 一个坐标系只有一个父系，但它可以有多个子系
• 如果我们要向tf添加一个新坐标系，三个现有坐标系中的一个需要是父系，新坐标系将成为子系

以乌龟为例子，添加一个新坐标系carrot1到turtle1

#include 
#include 

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，从父系turtle1到子系carrot1，carrot1离turtle1 两米远
    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;
};

9. 深入Time和TF⁹

• 坐标系树随时间变化而变化，tf存储每个变换的时间快照（默认情况下最多为10秒）
• 使用lookupTransform()函数来获取该tf树中最新的可用变换，使用now()来记录当前时间的变换

值得注意的是，如果只使用lookupTransform()在运行时程序会报错，这是因为 每个监听器有一个缓冲区，它存储来自不同tf广播者的所有坐标变换。 当广播者发出变换时，变换进入缓冲区之前需要一些时间（通常是几个毫秒），所以在时间“now”请求坐标系变换时等待几毫秒再获得该信息便解决了此问题。

 try{
    ros::Time now = ros::Time::now();
    listener.waitForTransform("/turtle2", "/turtle1", now, ros::Duration(3.0));
    listener.lookupTransform("/turtle2", "/turtle1",now, transform);

• waitForTransform() 四个参数：

1. 需要等待变换从坐标系turtle2
2. 到坐标系turtle1
3. 在now时间
4. 超时时间，不要等待超过此最大持续时间

注意：使用ros::Time::now()是为了这个例子。通常这将是希望被转换的数据的时间戳。waitForTransform()实际上会阻塞直到两个海龟之间的变换可用（这通常需要几毫秒）或者如果变换不可用,直到达到超时。

对于真实的tf用例，使用Time(0)通常是完全正常的。

10. 时间穿梭(Time travel)¹⁰

  try{
    ros::Time now = ros::Time::now();
    listener.waitForTransform("/turtle2", "/turtle1",
                              now, ros::Duration(1.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             now, transform);

同样以小乌龟Demo为例，现在，不是让turtle2去到turtle1当前时间的地方，而让turtle2去turtle1是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);

相对于/turtle2目前的位置，/turtle1 5秒前的姿势是什么?

回答该问题需要依赖于高级API,示例代码如下：

  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);

• 这个lookupTransform()API，有六个参数：

1. 变换从坐标系turtle2
2. 在now时间
3. 到turtle1坐标系
4. 在past时间
5. 指定不随时间改变的坐标系，这里是world
6. 变换结果保存的变量

• waitForTransform()跟lookupTransform()一样有6个相应参数。
  
  在上图中，在world坐标系,tf时间从past到now，past时间tf会计算从turtle1到world坐标系的变换，在now时间，tf计算从world到turtle2坐标系的变换

---

1. https://www.ncnynl.com/archives/201702/1305.html ↩︎

2. https://www.ncnynl.com/archives/201702/1306.html ↩︎

3. https://www.ncnynl.com/archives/201702/1307.html ↩︎

4. https://www.ncnynl.com/archives/201702/1308.html ↩︎

5. https://www.ncnynl.com/archives/201702/1309.html ↩︎

6. https://www.ncnynl.com/archives/201702/1310.html ↩︎

7. https://www.ncnynl.com/archives/201702/1311.html ↩︎

8. https://www.ncnynl.com/archives/201702/1312.html ↩︎

9. https://www.ncnynl.com/archives/201702/1313.html ↩︎

10. https://www.ncnynl.com/archives/201702/1314.html ↩︎