ROS的TF坐标变换
TF 坐标变换
坐标转换是一个坐标在不同坐标系下的表示，而坐标系转换是不同坐标系的相对位姿关系
ROS中机器人模型包含大量的部件，每一个部件统称之为link（比如手部、头部、某个关节、某个连杆）,每一个link上面对应着一个frame（坐标系）,
用frame表示该部件的坐标系，frame和link是绑定在一起的。
TF是一个通俗的名称，实际上它有很多含义
1、可以被当做是一种标准规范，这套标准定义了坐标转换的数据格式和数据结构．tf本质是树状的数据结构，即"tf tree"。
2、tf也可以看成是一个话题 /tf，话题中的消息保存的就是tf tree的数据结构格式。维护了整个机器人的甚至是地图的坐标转换关系。维持并更新机器人整个坐标系的话题是/tf，
/tf话题表示的内容是整个机器人的tf树，而非仅仅是某两个坐标系的转换关系，这样的话，/tf话题是需要很多的节点来维护的，每一个节点维护两个frame之间的关系。
3、tf还可以看成是一个package,它当中包含了很多的工具．比如可视化，查看关节间的tf,debug tf等等
4、tf含有一部分的API接口，用来节点程序中的编程。TF对发布器与订阅器进行了封装，使开发者通过TF的接口更加简单地建立对TF树中某些坐标系转换关系的维护与订阅
tf是一个树状结构，维护坐标系之间的关系，靠话题通信机制来持续地发布不同link之间的坐标关系。
作为树状结构，要保证父子坐标系都有某个节点在持续地发布他们之间的位姿关系，才能使树状结构保持完整。
只有父子坐标系的位姿关系能被正确的发布，才能保证任意两个frame之间的连通。
如果出现某一环节的断裂，就会引发error系统报错．所以完整的tf tree不能有任何断层的地方，这样我们才能查清楚任意两个frame之间的关系。
每两个相邻frame之间靠节点发布它们之间的位姿关系，这种节点称为broadcaster。
broadcaster就是一个发布器publisher,如果两个frame之间发生了相对运动，broadcaster就会发布相关消息。
使用TF主要两个部分
1、监听TF变换
接受并缓存系统中发布的所有坐标变换数据，并从中查询所需要的坐标变换关系
2、广播TF变换
向系统中广播坐标系之间的坐标变换关系，系统中可能会存在多个不同部分的TF变换广播，每个广播可以直接将坐标变换关系插入TF树中，不需要再进行同步
工具：
tf_monitor :打印TF树中所有坐标系的发布状态，也可以通过输入参数来查看指定坐标系之间的发布状态
rosrun tf tf_monitor #显示当前坐标变换树的信息，主要是名称和实时的时间延时
tf_echo :查看指定坐标系之间的变换关系
rosrun tf tf_echo source_frame target_frame
view_frames : 可视化调试工具，生成PDF文件，显示整个TF树信息
rosrun tf view_frames
这个工具首先订阅/tf，订阅5秒钟，根据这段时间接受到的tf信息，绘制成一张tf tree，然后创建成一个pdf图。
将会以图形的形式显示出TF树中所有的frame和两个frame 的父子关系及其Broadcaster、Average rate等
基于TF的乌龟运动跟随
roslaunch turtle_tf turtle_tf_demo.launch
运行后可以看到两只小乌龟，其中一直在朝着另一只移动
打开键盘控制节点，控制原本静止的乌龟进行移动
rosrun turtlesim turtle_teleop_key
查看此时的TF树
rosrun tf view_frames
可以看到当前系统存在三个坐标系world 、turtle1 、turtle2
world是世界坐标系，作为系统的基础坐标系，其他坐标系都相对于该坐标系建立。所以world坐标系是TF树的根节点，相对于world坐标系，两只乌龟分别建立了自己的坐标系，
这两个坐标系的原点就是乌龟在世界坐标系下的坐标位置
要让turtle2跟随turtle1移动，等价于让坐标系turtle2跟随 坐标系turtle1移动，这就需要知道turtle2于turtle1之间的坐标变换。
三者的坐标变换关系可用如下公式表示
Tturtle1_turtle2 = Tturtle1_world X Tworld_turtle2
使用tf_echo可以查看当前状态下两乌龟坐标系之间的变换关系
也可以通过rviz 图形界面更加形象看到这三者之间的坐标关系
rosrun rviz rviz -d rospack find turtle_tf /rviz/turtle_rviz.rviz
能够随时知道turtle2与turtle1之间的坐标变换后就能计算两乌龟之间的距离和角度，就能控制turtle2向turtle1移动
1、首先创建一个发布乌龟坐标系与世界坐标系之间TF变换的节点，实现源码code_learning/learning_tf/src/turtle_tf_broadcaster.cpp
transformBroadcaster()类就是一个publisher,而sendTransform的作用是来封装publish的函数。
在实际的使用中，我们需要在某个Node中构建tf::TransformBroadcaster类，然后调用sendTransform(),将transform发布到/tf的一段transform上。
#include <ros/ros.h> #include <tf/transform_broadcaster.h> #include <turtlesim/Pose.h> std::string turtle_name; void poseCallback(const turtlesim::PoseConstPtr& msg) { // tf广播器 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)); //利用广播将坐标变换插入tf树并进行发布 } 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]; // 订阅乌龟的pose信息 ros::NodeHandle node; ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback); //处理乌龟pose消息的回调函数 poseCallback ros::spin(); return 0; };
2、创建TF监听器
TF消息广播后，其节点就能监听到该消息，目前已经将乌龟于world的TF变换进行了广播，接下来就需要监听TF消息，从中获取turtle1与turtle2之间的坐标系变换，从而控制turtle2移动
code_learning/learning_tf/src/turtle_tf_listener.cpp
#include <ros/ros.h> #include <tf/transform_listener.h> #include <geometry_msgs/Twist.h> #include <turtlesim/Spawn.h> int main(int argc, char** argv) { // 初始化节点 ros::init(argc, argv, "my_tf_listener"); ros::NodeHandle node; // 通过服务调用，产生第二只乌龟turtle2 ros::service::waitForService("spawn"); ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("spawn"); turtlesim::Spawn srv; add_turtle.call(srv); // 定义turtle2的速度控制发布器 ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10); // tf监听器 tf::TransformListener listener; ros::Rate rate(10.0); while (node.ok()) { tf::StampedTransform transform; try { // 查找turtle2与turtle1的坐标变换 listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0)); listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform); } catch (tf::TransformException &ex) { ROS_ERROR("%s",ex.what()); ros::Duration(1.0).sleep(); continue; } // 根据turtle1和turtle2之间的坐标变换，计算turtle2需要运动的线速度和角速度 // 并发布速度控制指令，使turtle2向turtle1移动 geometry_msgs::Twist vel_msg; vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(), //设置turtle2速度控制msg的角速度 transform.getOrigin().x()); vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) + //设置turtle2速度控制msg的线速度 pow(transform.getOrigin().y(), 2)); turtle_vel.publish(vel_msg); rate.sleep(); } return 0; };
首先通过服务调用产生乌龟turtle2,然后申明控制turtle2速度的publisher,在监听TF消息前，需要先创建一个 tf::TransformListener 监听器，创建成功后监听器会自动接受TF树消息，并且缓存10秒
然后在循环中就可以实时查找TF树中的坐标变换了
两个重要接口：
waitForTransform 给定源坐标系和目标坐标系，等待两个坐标系指定时间（time）的变换关系,该函数会阻塞程序运行，所以需要设置超时时间（timeout）
lookupTransform 给定源坐标系和目标坐标系，得到两个坐标系指定时间（time）的变换（tramsform）,ros::Time(0)代表我们需要的是最新一次的坐标变换
launch文件 将所有节点配置运行起来，包括小海龟节点，键盘控制节点，两个小海龟的广播节点（分别广播自己与world坐标系之间的坐标变换），监听节点（通过服务调用第二个海龟，并控制该海龟的运动行为）
<launch>
<!-- 海龟仿真器 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- 两只海龟的tf广播 -->
<node pkg="learning_tf" type="turtle_tf_broadcaster"
args="/turtle1" name="turtle1_tf_broadcaster" />
<node pkg="learning_tf" type="turtle_tf_broadcaster"
args="/turtle2" name="turtle2_tf_broadcaster" />
<!-- 监听tf广播，并且控制turtle2移动 -->
<node pkg="learning_tf" type="turtle_tf_listener"
name="listener" />
</launch>
个人理解：TF提供给用户一个平台，只需要每个个体的坐标系在整个TF树中能连贯起来，那么就能实现TF树中任意两个体之间的坐标变换
