在机器人的控制中,坐标系统是非常重要的,在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);