ROS之tf空间坐标变换浅析 （二）

带你理解ros中TF的函数调用 二

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

---

tf

目的：

turtle1和turtle2都是world的子坐标系，carrot1是turtle1的子坐标系，就是 turtle1和turtle2以world参考系，carrot1是turtle1为参考系。
tf库可以根据和这个已知关系，计算出各个坐标系间的相互关系。
现在我们有两个目的，一个是做出让turtle2以turtle1为参考系的变化，另一是turtle2以carrot1为参考系的变化。

下面我们用例子来解释，但是我建议你看下我第一篇TF，因为里面讲到了函数的使用，这里仍然会使用。

下面的代码，按照http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28C%2B%2B%29
或者http://www.ncnynl.com/archives/201702/1310.html
构建，这里不说怎么构建，你可以去模仿。

先看一个简单字例子，turtle_tf_broadcaster.cpp，先给出广播代码，先看主函数，然后直接看分析。

#include 
#include 
#include 

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg){
  static tf::TransformBroadcaster br; //创建一个广播对象，它会把变化信息广播到tf中

  //创建变化信息，并且初始化。为什么/pose可以作为转化信息 ，因为它本身就是全局的，相对于原点的坐标
  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);
  //指出这个变换信息是turtle_name相对world的坐标信息，并且广播出去
  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];
  printf("%s",argv[1]);
  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
  ros::spin();
  return 0;
};

分析：
我们会用上面的代码广播两个位置信息，turtle1/pose和turtle2/pose，所以得启动这个节点两次。怎么启动不要急，我们先把这个代码分析清楚。首先turtle1和turtle2是通过argv传递过来，这是执行节点命令时输入的。同时我们也会启动乌龟节点，让它发布这个/pose话题，这个节点（my_tf_broadcaster）会去订阅，订阅到位置话题后，通过ros：：spin()调用poseCallback函数,让系统知道乌龟的位置。
如果你看懂我我之前一篇，回调函数里面的东西应该很好理解。现在看回调函数注释部分。关于TransformBroadcaster类前篇已经简单分析过。现在再分析一遍,监听函数仍然用到。

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){ };

  /** \brief Default constructor only to be used for preallocation */
  StampedTransform() { };

  /** \brief Set the inherited Traonsform data */
  void setData(const tf::Transform& input){*static_cast(this) = input;};

};

这个类StampedTransform储存着谁到谁的变换信息，相对数据储存在基类transform中。然后通过tf::TransformBroadcaster类中的函数sendTransform（）发送到tf中。

然后我们创建一个launch文件,命名start_demo.launch,用于启动节点。可以看出启动了乌龟节点和控制乌龟行走的键盘节点。还有两个广播变化节点。单独启动这个文件也没什么意义，待讲完监听节点也加入此文件，就明白了。

<launch>
    
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
    
    <param name="scale_linear" value="2" type="double"/>
    <param name="scale_angular" value="2" type="double"/>
    
    <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" />
  launch>

下面图只看到了turtle1相对于world的位置，因为只有一个乌龟节点启动了(< node pkg=”turtlesim” type=”turtlesim_node” name=”sim”/>)，没有第二只乌龟发布turtle2/pose。

---

下面是监听的代码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");  ////等待，直到服务"spawn"出现。只有这样才能请求此服务再产生一个turtle
  ros::ServiceClient add_turtle =
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn srv;
  add_turtle.call(srv);  //调用服务再产生一个turtle

  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

  tf::TransformListener listener;  //创建一个监听对象

  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
      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;
};

好，我们将监的节点 加入到launch文件中，我们来分析代码。

ros::service::waitForService("spawn");  ////等待，直到服务"spawn"出现。只有这样才能请求此服务再产生一个turtle
  ros::ServiceClient add_turtle =
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn srv;
  add_turtle.call(srv);  //调用服务再产生一个turtle

上面这段代码的是复制一个乌龟出来，在之前的launch文件中，我们只启动了一个乌龟，所以我们要参数服务器再启动一个乌龟。等价于命令行 rosservice call /spawn x y theta "turtle_name",后面我们将用这只乌龟发布turtle2/pose。到这里，如果把这个监听节点也运行，注释掉下面的东西，已经看见两只乌龟了。

tf::TransformListener listener; //创建一个监听对象
上面这句创建了一个监听对象，一旦监听器被创建，它开始接收tf转换，并缓冲它们长达10秒。

try{
listener.lookupTransform("/turtle2", "/turtle1",
ros::Time(0), transform);
}
上面这行代码是核心代码。我们知道类TransformListener是继承基类Transformer的。lookupTransform函数是在基类Transformer里，我们来看看它的定义

void lookupTransform(const std::string& target_frame, const std::string& source_frame,
const ros::Time& time, StampedTransform& transform) const;

分析功能：目的在最近time时间监听的可用变换，将target_frame坐标系变换到source_frame坐标系，说简单点就是计算出了这两个坐标点之间的距离和方向。结果保存到对象transform，其类StampedTransform已经讲过，可回头看。

现在通过这个lookupTransform（）函数已经得出/turtle2和turlte1的相对关系，并保存在了对象tarnform中。于是我们可以使用tarnform对象的基类函数getOrigin()获取坐标,是turtle2相对于turlte1的坐标数据。这样就得出了turtle2相对于turtle1的坐标变化，可自己输出来。

但是我们想要直观的看到感觉到这种变化。我们可以用它俩的相对数据干什么呢？我们可以让tuelte2跑到turtle1位置去。如果我们知道两个点的坐标，是不是可以算出它俩的方向？于是我们基于turltle2相对于turtle1的距离构成turtle2的速度，turltle2相对于turtle1的方向构成turtle2的方向，然后发布到话题turtle2/cmd_vel，这样turtle2就会订阅朝着turtle1爬去。当它俩重合的时候，距离为0，速度自然为0，turtle2就不能爬了。
所以，当我们移动turtle1时，turlte2会跟着跑，就是这个道理。

---

---

下面我们看看如何为空间新增一个坐标系corrot1，并且计算出turtle2相对于corrot1的位置关系。
注意：
其实我们要增加传感器或者改变原来传感器的位置，就要新增坐标系。tf会处理所引入的额外坐标系，且不会更改原来的坐标系统，tf坐标系树种不允许出现环，只能靠树的关系来建立，所以当你引入新坐标系一定要说明这个坐标的父系是谁和相对父系的坐标数据，然后tf才会处理它，最后可以通过tf库计算出它与任何坐标系的相对位置。这就是tf库的伟大。

最开始的那张图，tf树包含三个坐标系：world，turtle1和turtle2。 两只乌龟是世界的子系。
如果我们要向tf添加一个新坐标系，三个现有坐标系中的一个需要是父系，新坐标系将成为子系。我们将turtle1作为corrot1的父系。

加入新坐标代码：

#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) );
   //广播，指明相对关系和数据
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
    rate.sleep();
  }
  return 0;
};

是不是发现跟前面的广播代码是一样的，没错就是一样的。这里先创建一个变换对象transform，并且初始化。然后广播出去，指定carrot1的相对于父系turtle1的相对位置是tranform。这样就在坐标空间增加了新坐标carrot1。
编译，把这个节点也加入到上面那个launch文件中。然后运行。你会发现乌龟什么都没改变，你只是广播了新增的点，什么也没干。但是在rviz中可以看到新增的坐标，如图：移动turtle1，carrot1相对于turlte1的位置不变，相对世界点原点一直在变，而turtle2也一直追着turtle2在跑。

现在我们让turtle2跟着corrot1跑，只需要将lookupTransform()修改为

 listener.lookupTransform("/turtle2", "/carrot1",ros::Time(0), transform);

这样运行起来之后，turtle1移动，corrot1相对于turlte1不动，然后turtle2追着corrot1跑。

下面再说一个随着时间的移动，turtle2绕着turtle1绕圈。很简单，只要改变turlte1与carrot1的相对距离，这个相对距离随时间变化,carrot1绕着turtle1,turtle2追着carrot1，直观上自然就绕着turtle1绕圈了。
于是我们更改carrot1跟turtle1的距离：

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) );

看到没，对时间用正余弦然后当成距离。c1绕着t1转圈，t2追着c1，于是就形成了左边的乌龟轨迹，乌龟1不动，乌龟2绕着乌龟1转圈圈。此时我们还可以移动t1，轨迹就跟复杂了。

---

对这tf,里面还有很多可用的函数，目前我不需要用到这些，所以先了解到这，以后有机会再继续深入一下。