ROS之tf空间坐标变换浅析

首先tf空间坐标变化应该是ros中常用到的，所以简单熟悉了一下。我在学习过程挺痛苦的额，可能是基础不好。这一篇我是对照wiki网站和创客学习的，有不对的地方希望高手能指出。

tf

定义：

tf(Transform Frame)是软件库，它会管理坐标变换树。虽然可以用数学知识来完成，但太麻烦。我们可以向机器人添加很多传感器和组件，还可以更组件的位置，这样需要添加新坐标到变换树，怎么添加呢？我们可以直接调用tf库来完成。

下面我们用例子来解释。但是我建议看是把http://wiki.ros.org/tf知识先看一遍。

先看一个简单字例子，我们给出机器人base_link和base_laser的相对位置，然后创建一个相对于base_laser的点，输出这个点相对于base_link的位置。
广播代码：

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

int main(int argc, char** argv) {
    ros::init(argc, argv, "tf_publisher");
    ros::NodeHandle n;
    ros::Rate r(100);
    tf::TransformBroadcaster broadcaster;
    tf::Transform transform;
    transform.setOrigin(tf::Vector3(0.1, 0, 0.2));
    tf::Quaternion q;
    q.setRPY(0, 0, 0);
    transform.setRotation(q);
    while(n.ok()) {
        broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "base_laser"));  //发布了base_link和base_laser之间的坐标关系
        r.sleep();
    }

    return 0;
}

好，现在来看下TransformBroadcaster类，这个类最大的意义就是调用里面的函数方法，官方说是为方便，你知道就行。最主要的是方法参数StampedTransform，直接看下一个代码片StampedTransform类的构造。

namespace tf
{
class TransformBroadcaster{
public:

  TransformBroadcaster();
  void sendTransform(const StampedTransform & transform);
  void sendTransform(const std::vector<StampedTransform> & transforms);
  void sendTransform(const geometry_msgs::TransformStamped & transform);
  void sendTransform(const std::vector<geometry_msgs::TransformStamped> & transforms);
private:
  tf2_ros::TransformBroadcaster tf2_broadcaster_;
};
}

接下来看看class StampedTransform

class StampedTransform : public tf::Transform
{
public:
  ros::Time stamp_; ///< The timestamp associated with this transform
  std::string frame_id_; ///< The frame_id of the coordinate frame  in which this transform is defined
  std::string child_frame_id_; ///< The frame_id of the coordinate frame this transform defines
  StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id):
    tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ };
  StampedTransform() { };
};

这个类是继承基类tf::Transform，而基类里面放的数据是两个点的相对位置。下面这个初始化就是在说两个点的相对位置，包括方向和距离：

 tf::Transform transform;
    transform.setOrigin(tf::Vector3(0.1, 0, 0.2));
    tf::Quaternion q;
    q.setRPY(0, 0, 0);
    transform.setRotation(q);

相对位置的数据有了，可是没说谁与谁的相对位置，于是下面这句话就说出了base_laser相对base_link的相对位置，其中可以理解为base_link为父系，base_laser为子系，子系距离父系的距离和方向。并且广播出去了
“`
broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), “base_link”, “base_laser”));
到这里，应该对广播代码有一定的了解了。

下面来看看监听的代码。

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

void transformPoint(const tf::TransformListener &listener) {
    geometry_msgs::PointStamped laser_point;  //这里定义了一个相对于laser_link坐标系的点
    laser_point.header.frame_id="base_laser";
    laser_point.header.stamp=ros::Time();
    laser_point.point.x=1.0;     //设置相对于laser_link坐标系的坐标
    laser_point.point.y=2.0;
    laser_point.point.z=0;
    try{
        geometry_msgs::PointStamped base_point;
        listener.transformPoint("base_link", laser_point, base_point); //Transform a Stamped Point Message into the target frame

        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 form \"base_laser\" to \"base_link\": %s", ex.what());
    }
}

int main(int argc, char** argv){
    ros::init(argc, argv, "tf_listener");
    ros::NodeHandle n;
    tf::TransformListener listener(ros::Duration(10));
    //we'll transform a point once every second
    //typedef boost::function<void(const TimerEvent&)> TimerCallback;
    //Timer createTimer(Duration period, void(T::*callback)........
    //对自由方法来说，直接boost::bind(函数名, 参数1，参数2，...)
    //对类方法来说，直接boost::bind(&类名::方法名，类实例指针，参数1，参数2）
    ros::Timer timer=n.createTimer(ros::Duration(1.0),boost::bind(&transformPoint, boost::ref(listener)));
    ros::spin();
    return 0;
}

好，来分析，主函数定义了一个监听者，并且将监听的10S的数据记录下来。然后定义了一个定时器，调用回调函数，意思是每1S调用一下。来看下监听的类

class TransformListener : public Transformer { 
public:
  TransformListener(ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true);
  TransformListener(const ros::NodeHandle& nh,
                    ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true);

  ~TransformListener();
...
  void transformPoint(const std::string& target_frame, const geometry_msgs::PointStamped& stamped_in, geometry_msgs::PointStamped& stamped_out) const;
 ...
}

这个类 TransformListener也继承Transformer的。可见这里调用的是第一个构造函数，类里面有很多转化方法，这里列出了我们用的一个，点的转化。

  void transformPoint(const std::string& target_frame, const geometry_msgs::PointStamped& stamped_in, geometry_msgs::PointStamped& stamped_out) const;

意思是将相对于stamped_in坐标系的点转化到相对于target_frame坐标系，并将转化后的数据保存在stamped_out点里，这个stamped_out是相对的base_link坐标系的。现在应该理解监听代码了。
最后输出结果

base_laser:(1.00, 2.00, 0.00) -----> base_link:(1.10, 2.00,0.20) at time ...

接下来，我们来看下两个乌龟间的坐标转化，通过下一篇，相信你对坐标转化更加理解。