1.简介:
- tf是一个让用户随时间跟踪多个参考系的功能包,它使用一种树型数据结构,根据时间缓冲并维护多个参考系之间的坐标变换关系,可以帮助用户在任意时间,将点、向量等数据的坐标,在两个参考系中完成坐标变换。
- tf可以在分布式系统中进行操作,也就是说一个机器人系统中所有的参考系变换关系,对于所有节点组件,都是可用的,所有订阅tf消息的节点都会缓冲一份所有参考系的变换关系数据,所以这种结构不需要中心服务器来存储任何数据。
- 想要使用tf功能包,总体来讲可以分为以下两个步骤: 监听tf变换( 接收并缓存系统中发布的所有参考系变换,并从中查询所需要的参考系变换。) 广播tf变换 ( 向系统中广播参考系之间的坐标变换关系。系统中更可能会存在多个不同部分的tf变换广播,每个广播都可以直接将参考系变换关系直接插入tf树中,不需要再进行同步。)
2.tf介绍
2.tf tool
2.1 . 使用 view_frames
可视化的调试工具,可以生成pdf文件,来显示整棵tf树的信息。
创建一个在ROS上,由tf发布的坐标系的图片。
- $ rosrun tf view_frames #生成图片
- $ evince frames.pdf #查看图片
2.2 . 使用rqt_tf_tree
这是一个实时工具,观察在Ros上被发布的坐标系树,可用刷新按钮来更新树的内容。与上一个工具的区别在于:上一个工具连续采样5s获得的树的内容,并存成一个图片;这个工具可以连续的观察树的内容,使用起来更方便。
- $ rosrun rqt_tf_tree rqt_tf_tree
2.3 . 使用tf_echo
报告在ROS上任意两个坐标系发布的变换,查看指定参考系之间的变换关系。
- tf_echo <source_frame> <target_frame>
或者
- $ rosrun tf tf_echo [reference_frame] [target_frame]
2.4 . rviz和tf
rivz是一个可视化工具,用于检查tf坐标系。
- $ rosrun rviz rviz -d `rospack find package_name` /rviz/file_name.rviz
package_name是将要在rviz中观察内容所在的包的名字;file_name是.rviz文件的名称。
2.5. tf_monitor
tf_monitor工具的功能是打印tf树中的所有参考系信息,通过输入参数来查看指定参考系之间的信息。
- tf_monitor
- tf_monitor <source_frame> <target_target>
2.6 static_transform_publisher
static_transform_publisher工具的功能是发布两个参考系之间的静态坐标变换,两个参考系一般不发生相对位置变化。
- static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms
- static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms
以上两种命令格式,需要设置坐标的偏移和旋转参数,偏移参数都使用相对于xyz三轴的坐标位移,而旋转参数第一种命令格式使用以弧度为单位的 yaw/pitch/roll三个角度(yaw是围绕x轴旋转的偏航角,pitch是围绕y轴旋转的俯仰角,roll是围绕z轴旋转的翻滚角),而第二种命令格式使用四元数表达旋转角度。发布频率以ms为单位,一般100ms比较合适。
该命令不仅可以在终端中使用,还可以在launch文件中使用,使用方式如下:
<launch>
<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 link1_parent link1 100" />
</launch>
3. tf 的接听和发布
接听tf
整个demo
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <turtlesim/Velocity.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 srv;
add_turtle.call(srv);
ros::Publisher turtle_vel =
node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 10);
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok()){
tf::StampedTransform transform;
try{
ros::Time now = ros::Time::now();
listener.waitForTransform("/turtle2", "/turtle1",
now, ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1",
now, transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
turtlesim::Velocity vel_msg;
vel_msg.angular = 4.0 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
详解
tf包提供了TransformListener的实现,以帮助简化接收转换的任务。要使用TransformListener,我们需要包含tf/transform_listener.h头文件。
- #include <tf/transform_listener.h>
在这里,我们创建一个TransformListener对象。 一旦创建了监听器,它就开始通过线路接收转换,并将它们缓冲最多10秒钟。 TransformListener对象应限定为持久化,否则它的缓存将无法填充,几乎每个查询都将失败。 一种常见的方法是使TransformListener对象成为类的成员变量。
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
- 我们想要从frame / turtle1到frame / turtle2的转换。
- 我们想要转变的时间。提供ros :: Time(0)将为我们提供最新的变换。
- 我们存储结果转换的对象。
2.发布
整个demo
#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 transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
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;
};
详解
头文件
- #include <tf/transform_broadcaster.h>
发布的对象
- static tf::TransformBroadcaster br;
在这里,我们创建一个TransformBroadcaster对象,稍后我们将使用该对象通过线路发送转换。
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);
- br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
- 首先,我们传递变换本身。
- 现在我们需要为变换发布一个时间戳,我们只用当前时间标记它,ros :: Time :: now()。
- 然后,我们需要传递我们正在创建的链接的父框架的名称,在本例中为“world”
- 最后,我们需要传递我们正在创建的链接的子框架的名称,在这种情况下,这是乌龟本身的名称。
4. tf2的接听和发布
f2是变换库的第二代,它允许用户随时间跟踪多个坐标框架。 tf2维持在时间缓冲的树结构中的坐标帧之间的关系,并允许用户在任何期望的时间点在任何两个坐标帧之间变换点,向量等。
4.1接听
整个demo
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf2_listener");
ros::NodeHandle node;
ros::service::waitForService("spawn");
ros::ServiceClient spawner =
node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn turtle;
turtle.request.x = 4;
turtle.request.y = 2;
turtle.request.theta = 0;
turtle.request.name = "turtle2";
spawner.call(turtle);
ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10.0);
while (node.ok()){
geometry_msgs::TransformStamped transformStamped;
try{
transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",
ros::Time(0));
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transformStamped.transform.translation.y,
transformStamped.transform.translation.x);
vel_msg.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) +
pow(transformStamped.transform.translation.y, 2));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
详解:
头文件
- #include <tf2_ros/transform_listener.h>
定义对象
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
在这里,我们创建一个TransformListener对象。 一旦创建了监听器,它就会开始通过线路接收tf2转换,并将它们缓冲最多10秒钟。 TransformListener对象应限定为持久化,否则它的缓存将无法填充,几乎每个查询都将失败。 一种常见的方法是使TransformListener对象成为类的成员变量。
4.2 发布
整个demo
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg){
static tf2_ros::TransformBroadcaster br;
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "world";
transformStamped.child_frame_id = turtle_name;
transformStamped.transform.translation.x = msg->x;
transformStamped.transform.translation.y = msg->y;
transformStamped.transform.translation.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, msg->theta);
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();
br.sendTransform(transformStamped);
}
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf2_broadcaster");
ros::NodeHandle private_node("~");
if (! private_node.hasParam("turtle"))
{
if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
turtle_name = argv[1];
}
else
{
private_node.getParam("turtle", turtle_name);
}
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
};
5. message_filters::Subscriber & tf::MessageFilter
5.1 案例:
tf_ = new TransformListenerWrapper();
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
scan_filter_->registerCallback(boost::bind(&SlamGMapping::laserCallback, this, _1));
- message_filters,顾名思义是消息过滤器;tf::MessageFilter,顾名思义是tf下的消息过滤器。消息过滤器为什么要用tf呢?tf::MessageFilter可以订阅任何的ROS消息,然后将其缓存,直到这些消息可以转换到目标坐标系,然后进行相应的处理(一般在回调函数中处理)。
- 消息订阅+坐标转换。实际上,后者继承于前者:
5.2 头文件
#include "tf/transform_listener.h"
#include "tf/transform_broadcaster.h"
#include "message_filters/subscriber.h"
#include "tf/message_filter.h"
- 定义数据:TransformListener、message_filters::Subscriber、tf::MessageFilter
- 用消息的名称来初始化message_filters::Subscriber
- 用tf、message_filters::Subscriber、目标坐标系来初始化tf::MessageFilter
- 给tf::MessageFilter注册callback
- 编写callback,并在回调中完成坐标转换。至此完成消息订阅+坐标转换
6.tf的两种接听形式:
通过监听tf,我们可以避免繁琐的旋转矩阵计算,而直接获取我们需要的相关信息。在监听中最常用的又两个函数
(1)lookupTransform()
(2)transformPoint();
6.1 lookupTransform()
函数功能:可以获得两个坐标系之间转换的关系,包括旋转和平移。主要步骤有:
- 定义监听器 tf::TransformListener listener
- 定义存放变换关系的变量 tf::StampedTransform transform;
- 监听两个坐标系之间的变换,实现base_link到map的转换
try{
listener.lookupTransform("base_link", "map",
ros::Time(0), transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
6.2 tf_.transformPose()
bool Costmap2DROS::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{
global_pose.setIdentity();
tf::Stamped < tf::Pose > robot_pose;//定义一个位姿变量用来存储坐标值
robot_pose.setIdentity();//位姿各成员值初始化为0
robot_pose.frame_id_ = robot_base_frame_;//定义需要获得在那个坐标系下的坐标
robot_pose.stamp_ = ros::Time();
// get the global pose of the robot
tf_.transformPose(global_frame_, robot_pose, global_pose);//获取global_pose在robot_base_frame_坐标系下的坐标
}
- 这里只需要指定global_pose和robot_pose各自的frame_id,就可已通过函数tf_.transformPose(global_frame_, robot_pose, global_pose); 获得 global_pose 。
注意
- 由于tf会把监听的内容存放到一个缓存中,然后再读取相关的内容,而这个过程可能会有几毫秒的延迟,也就是,tf的监听器并不能监听到”现在“的变换,所以不使用try,catch函数会导致报错,并且会导致程序挂掉,使用try,catch之后就ok了。
- 不可以把ros::Time(0)改成ros::time::now(),因为监听做不到实时,会有几毫秒的延迟。ros::Time(0)指最近时刻存储的数据。最后转换关系存入transform中
6.3 扩展 transformPoint()
在实际应用中我们肯定会需要在一个坐标系下的点转换到另一个坐标系下,这就需要transformPoint()函数。
1.函数功能:将一个坐标系下的点的坐标转换到另一个坐标系下。
2.函数参数,如
listener_.transformPoint("map",laser_pose,map_pose);
(1)其中laser_pose,world_pose的数据类型都是 geometry_msgs::PointStamped, 需要定义laser_pose.header.frame.id即该点所属的坐标系(比如激光坐标系)
(2)”map“是指,我要将aser_pose转换到map坐标系下,map_pose是转换的结果。