TF package 介绍

最新推荐文章于 2024-07-18 22:53:34 发布

Lucifer_zhang

最新推荐文章于 2024-07-18 22:53:34 发布

阅读量1.7k

点赞数

分类专栏： Robot_Ros

本文链接：https://blog.csdn.net/seawolfe/article/details/69525176

版权

Robot_Ros 专栏收录该内容

4 篇文章 0 订阅

订阅专栏

在机器人的控制中，坐标系统是非常重要的，在ROS使用tf软件库进行坐标转换。

1 TF 简介

tf是一个让用户随时间跟踪多个参考系的功能包，它使用一种树型数据结构，根据时间缓冲并维护多个参考系之间的坐标变换关系，可以帮助用户在任意时间，将点、向量等数据的坐标，在两个参考系中完成坐标变换。

一个机器人系统通常有很多三维的参考系，而且会随着时间的推移发生变化，例如全局参考系（worldframe），机器人中心参考系（baseframe），机械夹参考系（gripperframe），机器人头参考系（headframe）等等。tf可以以时间为轴，跟踪这些参考系（默认是10秒之内的），并且允许用户提出如下的申请：

五秒钟之前，机器人头参考系相对于全局参考系的关系是什么样的？

机器人夹取的物体相对于机器人中心参考系的位置在哪里？

机器人中心参考系相对于全局参考系的位置在哪里？

tf可以在分布式系统中进行操作，也就是说一个机器人系统中所有的参考系变换关系，对于所有节点组件，都是可用的，所有订阅tf消息的节点都会缓冲一份所有参考系的变换关系数据，所以这种结构不需要中心服务器来存储任何数据。

想要使用tf功能包，总体来讲可以分为以下两个步骤：

（1）监听tf变换

接收并缓存系统中发布的所有参考系变换，并从中查询所需要的参考系变换。

（2）广播tf变换向系统中广播参考系之间的坐标变换关系。系统中更可能会存在多个不同部分的tf变换广播，每个广播都可以直接将参考系变换关系直接插入tf树中，不需要再进行同步。

2 tf tools

tf提供了丰富的命令行工具来帮助用户调试和创建tf变换。

2.1view_frames

view_frames是可视化的调试工具，可以生成pdf文件，来显示整棵tf树的信息。命令行的执行方式如下：

$rosrun tf view_frames

$evince frames.pdf

2.2rqt_tf_tree

rqt_tf_tree是tf树状结构的可视化运行工具，命令行的执行方式如下：

$rosrun rqt_tf_tree rqt_tf_tree

2.3tf_echo

tf_echo工具的功能是查看指定参考系之间的变换关系。

命令的格式如下：

$tf_echo <source_frame> <target_frame>

2.4tf_monitor

tf_monitor工具的功能是打印tf树中的所有参考系信息，通过输入参数来查看指定参考系之间的信息。

命令格式如下：

$tf_monitor

$tf_monitor <source_frame> <target_target>

3 Broadcasters

以下代码发布正在改变的乌龟的坐标系。

turtle_tf_broadcaster.cpp:

#include <ros/ros.h>

#include <tf/transform_broadcaster.h>

//tf包提供了一个实现机制TransformBroadcaster来帮助使得发布变换更容易，为了使用//TransformBroadcaster，我们需要包含这个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) );

//我们创建了一个Transform对象，并且复制这个乌龟图形的2D位置并转换为3D位置，添加了一个z系，但是设为0.0,即在rviz中也是在平面

tf::Quaternionq;

  q.setRPY(msg->theta, 0, 0);

  transform.setRotation(q);

//通过transform设置方位

 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

//定义了一个名称为br的TransformBroadcaster，即上面所提到的，是tf包中的，通过TransformBroadcaster来发布信息需要四点:
//1.首先需要transform
//2.需要给transform一个时间戳，这个时间戳是现在的时间
//3.然后，我们需要将我们创建的link的父框架的名字传输过去，在这个例子中是world
//4.最后，我们需要将我们创建的link的子框架的名字传输过去，在这里就是turtle本身
 }
//我们创建了一个TransformBroadcaster对象，我们之后会使用它来发布转换消息
 int main(int argc, char** argv){  ros::init(argc, argv, "my_tf_broadcaster");
//初始化ros,命名节点为my_tf_broadcaster
 if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};  turtle_name = argv[1];
//乌龟的名字可以输入 
 ros::NodeHandle node;
//NodeHandle是与ROS系统交流的最主要的接入点，是一个句柄
 ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
//从master订阅某乌龟的“/pose”话题，当消息到来时，即当乌龟位置改变时产生新的消息时，ROS将会调用poseCallback 
 ros::spin(); 
 return 0;};

总结：从.launch文件中启动了四个节点，分别是turtlesim包中的类型为turtlesim_node的节点sim，turtlesim中类型为turtle_teleop_key的节点teleop,还有刚才建的包learning_tf中类型为turtle_tf_broadcaster的两个节点turtle1_tf_broadcaster和turtle2_tf_broadcaster.当运行.launch文件后生成了一个乌龟turtle1,当利用键盘控制使乌龟移动时，乌龟的位置发生变化，并通过主题turtle1/pose来发布位置改变的消息，由于节点turtle1_tf_broadcaster订阅了这个主题，当这个主题有消息发布时（比如乌龟位置变化），则会调用poseCallback，正如上面分析所示，这个方法将会将消息反馈回来，反馈的消息将2D改为3D，反馈为Translation和Rotation,其中Rotation包括Quaternion和RPY. 这样，当乌龟移动时能够反馈位置信息并能够在终端看到，主要利用了订阅pose主题，并利用TransformBroadcaster来发布转换消息。

4 Listeners

turtle_tf_listener.cpp:

#include <ros/ros.h>

#include <tf/transform_listener.h>

#include <geometry_msgs/Twist.h>

//消息类型头文件

#include <turtlesim/Spawn.h>

//生成乌龟的头文件spawn turtle

int main(int argc, char** argv){

  ros::init(argc, argv, "my_tf_listener");

//初始化ROS

  ros::NodeHandle node;

//NodeHandle是与ROS交流的最主要的接入点

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

//通过主题turtle2/cmd_vel来发布geometry_msgs::Twist类型的消息

  tf::TransformListener listener;

//在这里我们创建了一个TransformListener名称为listener,一旦listener创建后，它开始就接收tf

//的转换消息并让它们缓冲10s.TransformListener应该要一直存在，否则它的缓存将不能填满并且几乎//所有的查询将不能进行

 ros::Rate rate(10.0);

  while (node.ok()){

    tf::StampedTransform transform;

    try{

      listener.lookupTransform("/turtle2", "/turtle1",  ros::Time(0), transform);

//我们查询listener为一个指定的转换，有四个主要参数：

//1，2我们想要转换从turtle2到turtle1,也即最初运行结果所看到的当turtle2移动时，turtle1跟随着turtle2的轨迹移动

//3转换的时间

//4用transform来存储转换结果

//使用模块try-catch

    catch (tf::TransformException &ex) {

      ROS_ERROR("%s",ex.what());

      ros::Duration(1.0).sleep();

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

//transform用来计算新的线性和角度的向量值为turtle2,这是基于turtle2到turtle1的距离和角度，//新的向量值将会发布在主题turtle2/cmd_vel上，sim乌龟将会使用它并更改turtle2的移动

  turtle_vel.publish(vel_msg);

    rate.sleep();

  return 0;

};

总结：这是用rqt看到的节点之间的通信图，当.launch运行时，生成了一个sim节点即生成一只乌龟，这个乌龟可以用teleop节点即按键控制它移动，注意上图，是通过主题/turtle1/cmd_vel来发布消息，/sim来接收消息来使用的，launch还产生了节点/turtle1_tf_broadcaster和/turtle2_tf_broadcaster,当乌龟1的位置改变时，节点/turtle1_tf_broadcaster和/turtle2_tf_broadcaster通过订阅/turtle1/pose和/turtle2/pose来得到位置转换消息，并将消息发布出来通过主题/tf，节点listener通过订阅主题/tf来的到位置转换消息，并将消息改变一定的linear和anguar发布到主题/turtle2/cmd_vel,而乌龟2订阅了这个主题，并随着这个命令来改变位置变换。即从上可知当乌龟1接收按键消息改变位置时，乌龟2能够接收到乌龟1改变的消息，并能够按照乌龟1位置的改变来改变自己的位置，这样就出现了这样的结果，乌龟2跟随这乌龟1移动。主要通过turtle_tf_broadcaster订阅位置改变消息并通过tf主题发布然后listener来订阅tf主题计算出位置改变消息并将消息通过主题/turtle2/cmd_vel发送给turtuer2。其中重点是位置转换使用tf。

5 Add a frame

在很多应用中，添加一个参考系是很有必要的，比如在一个world参考系下，有很一个激光扫描节点，tf可以帮助我们将激光扫描的信息坐标装换成全局坐标。

tf中的信息是一个树状的结构，world参考系是最顶端的父参考系，其他的参考系都需要向下延伸。如果我们在上文的基础上添加一个参考系，就需要让这个新的参考系成为已有三个参考系中的一个的子参考系。

以turtle1作为父参考系，建立一个新的参考系“carrot1”。代码如下：

#include <ros/ros.h>

#include <tf/transform_broadcaster.h>

intmain(intargc, char** argv){

  ros::init(argc, argv, "my_tf_broadcaster");

  ros::NodeHandlenode;

  tf::TransformBroadcasterbr;

  tf::Transformtransform;

  ros::Raterate(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"));

//我们新建一个从父坐标系turtle1到新的子坐标系carrot1的transform，其中carrot1在turtle1的左边2米处

    rate.sleep();

  return0;

};

打开src/turtle_tf_listener.cpp，把turtle1改为carrot1

listener.lookupTransform("/turtle2", "/carrot1",

                               ros::Time(0), transform);

Launch文件中添加：

<launch>

...

    <node pkg="learning_tf" type="frame_tf_broadcaster"

          name="broadcaster_frame" />

  </launch>

上述坐标系是固定的，若定义坐标系时代码修改为

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

则可建立一个运动的坐标系。

6 tf and time

ros::Time(0)表示最近的时间戳

ros::Time::now()表示当前时刻，可能会出错，因为每个listener都有一个缓冲器用来存储所有来自不同tf的broadcaster的坐标系，当一个broadcaster发送一个转换时，转换消息进入缓冲器之前需要一段时间（通常只有几毫秒）。所以当你需要一个转换消息的时间是now即现在时，你应该要等待消息到来的那几个毫秒。

此时需要添加waitForTransform()语句，waitForTransform将会延迟一段时间直到两个turtle能运行（这个语句会消耗几毫秒）。

也可以让第二只乌龟跟着第一只乌龟五秒前的轨迹运动，代码如下

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