tf包：
tf是一个包，使用户可以实时追踪多个坐标系。
tf实时的维持着树状缓冲结构中坐标系之间的相对关系，
使得用户可以在任意时刻、在任意两个坐标系之间进行进行点、向量等的转换。
1.一个典型的机器人系统有许多实时变化的3D坐标系，例如：world frame, base frame, gripper frame, head frame, etc。
tf实时追踪这些坐标系，使用户能够知道：
5s前，head frame相对于would frame在哪？
gripper中的物体相对于我的base的姿态？
base坐标系在map坐标系中的当前姿态？
tf可以运行在分布式系统中，这意味着坐标系的所有信息在系统的任何组件中都可以获取到。
从本质上来说tf用户有两个任务：监听tf、广播tf。
监听tf：接收并缓存系统广播的所有的坐标系，查询指定坐标系间的转换tf。
广播tf：发布坐标系相对于系统其它坐标系的姿态。
一个系统可以有多个广播器，每一个都提供了系统不同部分的信息。
2.tf命令行工具：
（1）tf_monitor：打印当前坐标系tf树信息到控制台。
格式：rosrun tf tf_monitor
例：rosrun tf tf_monitor //查看所有的tf
例：//查看/base_footprint坐标系到/odom坐标系的tf。
rosrun tf tf_monitor /base_footprint /odom
（2）tf_echo：打印出源坐标系到目标坐标系间的tf。
格式：rosrun tf tf_echo
（3）static_transform_publisher：静态tf发布器。
格式一：static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms
以周期period_in_ms发布frame_id到child_frame_id的偏移量为x y z yaw pitch roll的tf。
格式二：static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms
以周期period_in_ms发布frame_id到child_frame_id的偏移量为x y z qx qy qz qw的tf。
static_transform_publisher在.launch文件中也可以使用：
例：
args="1 0 0 0 0 0 1 link1_parent link1 100" />
（4）view_frames：图形化调试工具，可以产生当前tf tree的PDF.
例：$ rosrun tf view_frames && evince frames.pdf
（5）roswtf：这是一个插件，分析当前tf的配置，试图找出常见的问题。
3.写一个tf广播器：
（1）创建tf广播包，依赖tf roscpp rospy turtlesim。
mkdir workspace/src && cd workspace/src
catkin_create_pkg learning_tf tf roscpp rospy turtlesim
cd workspace && catkin_make && source devel/setup.bash
（2）创建tf广播源代码，广播小海龟移动时坐标系的变化。
roscd learning_tf && vim src/turtle_tf_broadcaster.cpp
粘贴以下代码：
#include
#include
#include
std::string turtle_name;
//姿态回调函数
void poseCallback(const turtlesim::PoseConstPtr& msg){
static tf::TransformBroadcaster br; //广播器
tf::Transform transform; //tf
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );//tf位置
tf::Quaternion q; //单位四元数
q.setRPY(0, 0, msg->theta); //角度
transform.setRotation(q); //tf的旋转
//发布tf=transform，时间=ros::Time::now()，父坐标系="world"，子坐标系=turtle_name
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;
};
（3）编译：
CMakelists.txt中添加：
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
$catkin_make
将会在devel/lib/learning_tf文件夹下产生turtle_tf_broadcaster二进制文件。
（4）编写launch文件start_demo.launch。
args="/turtle1" name="turtle1_tf_broadcaster" />
args="/turtle2" name="turtle2_tf_broadcaster" />
（5）查看结果：
$ roslaunch learning_tf start_demo.launch
$ rosrun tf tf_echo /world /turtle1
4.编写一个tf监听器：
（1） 监听器源码：$ roscd learning_tf
vim src/turtle_tf_listener.cpp
粘贴以下代码：
#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
turtlesim::Spawn srv;
add_turtle.call(srv);
ros::Publisher turtle_vel = node.advertise
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok()){
tf::StampedTransform transform;
try{
//最新的从"/turtle2"到"/turtle1"的tf存放在transform中。
listener.lookupTransform("/turtle2", "/turtle1",ros::Time(0), transform);
}
catch (tf::TransformException &ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
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;
};
（2）在CMakelists.txt中添加：
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
在start_demo.launch文件中添加：
...
name="listener" />
（3）运行：
$ roslaunch learning_tf start_demo.launch
5.添加一个一个固定坐标系：
tf构建一个坐标系树状架构，架构中不允许出现闭环。这意味着一个坐标系只有一个父坐标系，但可以有多个子坐标系。
现在我们拥有三个坐标系，一个父坐标系would，两个子坐标系turtle1、turtle2。
我们想要添加坐标系，那它的父坐标系需要从三个中选一个。
现在我们新建一个坐标系，它的父坐标系是turtle1，它将成为turtle2的诱饵。
$ roscd learning_tf
$ vim src/frame_tf_broadcaster.cpp
（1） 粘贴源代码：
#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.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );//左移2米
transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
//广播由"turtle1"经transform得到"carrot1"
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
rate.sleep();
}
return 0;
};
（2）CMakeLists.txt粘贴：
add_executable(frame_tf_broadcaster src/frame_tf_broadcaster.cpp)
target_link_libraries(frame_tf_broadcaster ${catkin_LIBRARIES})
start_demo.launch中粘贴：
...
（3）打开src/turtle_tf_listener.cpp替换turtle1为carrot1的监听：
//第二只乌龟开始跟随诱饵carrot1，carrot1不可见，但存在。
listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0), transform);
（4）测试：
$ catkin_make
$ roslaunch learning_tf start_demo.launch
$ roslaunch learning_tf start_demo.launch
6.添加一个tf变化的坐标系：
（1）将5中的源代码修改为：
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) );
（2）测试：
$ catkin_make
$ roslaunch learning_tf start_demo.launch
7.学习waitForTransform函数：等待一个tf tree中指定transform。
tf tree随时间发生变化，tf存储每个transform一段时间内的快照（默认最多10秒）。
我们采用lookupTransform()函数获取tf tree中请求的最新的transform，但不知道这个transforme什么时候录制的。
这个教程将告诉你如何获取指定时间的transform。
（1）例： try{
listener.lookupTransform("/turtle2", "/turtle1", ros::Time::now(), transform);
}
报错：You requested a transform that is 0.018 miliseconds in the past,
but the most recent transform in the tf buffer is 7.681 miliseconds old.
每个listener有一个buffer，这个buffer中存储了来自不同tf broadcasters的所有的transform。
当一个transform发出后，在它到达buffer的过程中，它需要花费一些时间（）通常是几个毫秒。
所以你请求的now时刻的transform，需要等待几毫秒后才能到达。
（2）使用waitForTransform函数解决（1）中的问题。
try{
ros::Time now = ros::Time::now();
//从now时刻起等待从"/turtle2"到"/turtle1"的transform变换ros::Duration(3.0)时间。
listener.waitForTransform("/turtle2", "/turtle1",now, ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1",now, transform);
}
waitForTransform()将会阻塞直到需要的transform到来，或者超时。
（3）测试：
$ catkin_make
$ roslaunch learning_tf start_demo.launch
还可能出现如下错误：You requested a transform that is 3.009 seconds in the past,
but the tf buffer only has a history of 2.688 seconds.
这种情况发生的原因是turtle2产生和发布tf坐标系花费了一些时间。
所以你第一次请求/turtle2坐标系时，可能它还没产生，所以transform也就不存在。
第一次transform之后，所有的transform都存在了。
8.tf的时间传送特性。
将7中的代码片段替换为：
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);
}
这会使得第二只乌龟去往第一只乌龟5秒之前去过的地方。
lookupTransform（）的6个参数：
now时刻的"/turtle2"坐标系到past时刻的"/turtle1"坐标系的transform，
/would:这期间不发生改变的坐标系。transform：存储变换的变量。
$ make or catkin_make
$ roslaunch learning_tf start_demo.launch
