ROS关于tf坐标转换的问题（二）

1、tf变换的主要目的就是统一机器人的所有坐标关系，它可以实现系统中任一个点在所有坐标系之间的坐标变换。
tf分为两个部分：
（1）监听tf变换：接收并缓存系统中发布的所有参考系变换，并从中查询所需要的参考系变换。
（2）广播tf变换：向系统中广播参考系之间的坐标变换关系。系统中更可能会存在多个不同部分的tf变换广播，每个广播都可以直接将参考系变换关系直接插入tf树中，不需要再进行同步。

如果你足够了解机器人的工作机制，通过查阅机器人话题会发现tf应用很广泛，而tf的api数据类型如下面几种形式出现：

数据类型						对应TF
Quaternion     			tf::Quaternion
Vector						tf::Vector3
Point  						tf::Point
Pose						tf::Pose
Transform				tf::Transform

Quaternion 是表示四元数，vector3是一个3*1 的向量，point是一个表示一个点坐标，Pose是位姿（位姿是包括坐标以及方向） Transform是一个转换的模版。
2、我们借助ros wiki的图解来帮助我们了解具体请先看（(http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF)）

注意该图的laser坐标和base_link坐标。
变换关系如下：

有了上面的理论支撑我们可以进行下一步的说明。

3、我们以激光雷达的坐标laser和机器人坐标不base_link为例来讲，做机器人导航时也需用到该tf转换。
我们先编写广播节点：

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
        ros::init(argc, argv, "robot_tf_publisher");
        ros::NodeHandle n;

        ros::Rate r(100);

        tf::TransformBroadcaster broadcaster;

        while(n.ok()){
                broadcaster.sendTransform(
                tf::StampedTransform(
                tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
                ros::Time::now(),"base_link", "laser"));
                r.sleep();
        }
}

分析主要程序：通过TransformBroadcaster 类进行发布TF转换关系，注意实现的格式，类中的第一个参数Quaternion（x，y，z，w）这四个参数都是角度信息（具体信息请自行查找）;第二个参数为Vector3为坐标之间的关系转换。第四个参数是父系参考坐标系，即base_link，最后一个参数是子节点参考系，laser。

4、编写监听节点：

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>

void transformPoint(const tf::TransformListener& listener){
        //we'll create a point in the laser frame that we'd like to transform to the base_link frame
        geometry_msgs::PointStamped laser_point;
        laser_point.header.frame_id = "laser";

        //we'll just use the most recent transform available for our simple example
        laser_point.header.stamp = ros::Time();

        //just an arbitrary point in space
        laser_point.point.x = 1.0;
        laser_point.point.y = 0.2;
        laser_point.point.z = 0.0;

        try{
                geometry_msgs::PointStamped base_point;
                listener.transformPoint("base_link", laser_point, base_point);

                ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
                laser_point.point.x, laser_point.point.y, laser_point.point.z,
                base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
        }

        catch(tf::TransformException& ex){
                ROS_ERROR("Received an exception trying to transform a point from \"laser\" to \"base_link\": %s", ex.what());
        }
}

int main(int argc, char** argv){
        ros::init(argc, argv, "robot_tf_listener");
        ros::NodeHandle n;

        tf::TransformListener listener(ros::Duration(10));

        //we'll transform a point once every second
        ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

        ros::spin();

}

程序解析：上述用到tf::TransformListener对象，该对象会自动订阅ROS中的tf消息，并且管理所有的变换关系数据。在void transformPoint回到函数里，我们完成了坐标转换问题，从laser到base_link的转换。geometry_msgs::PointStamped laser_point;这个类就是实际中激光雷达所包含的信息数据，包括id，time。。
注意：该函数是关键transformPoint，它的第一个参数是需要转换到的参考系id，是base_link；第二个参数是需要转换的原始数据（激光雷达探测的数据）；第三个参数转换完成的数据。base_point就是转换后的坐标！！！

上面是C++版本，想要python版本的请自行查看官网教程：(http://wiki.ros.org/tf/Tutorials)