ros tf

原文：http://blog.csdn.net/start_from_scratch/article/details/50762293
tf是ROS中建立坐标系，并且使用各个坐标间转换关系的一个很好的工具，对于非导航专业的同学，常常苦恼与各种旋转矩阵的变换，自己经常被搞的头大，最近由于课题实验的需要，尝试使用tf，对此有了一个初步的了解。很多已经接触ROS有一段时间的朋友，可能都没有真正使用过tf,因为从官方给的例程看起来好像非常繁琐，所以也就不想使用，但是最近一段时间的学习发现，其实真正有用程序的就仅仅几句，所以其实非常简单。

一．广播tf

其实就是发布你建立的坐标系。步骤如下：
1.定义一个广播，相当于发布话题时定义一个发布器，还是以官方的小乌龟例程为例：
http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28C++%29

static tf::TransformBroadcaster br；

2.定义存放转换信息（平动，转动）的变量

tf::Transform transform;

3.设置坐标原点

transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );

需要注意：
（1）setOrigin()函数的参数类型需要为tf::Vector3类型
（2）假设是要发布一个子坐标系为”turtle1”父坐标系为“world”，那么其中(msg->x,msg->y,0.0)是指“turtle1”的坐标原点在“world”坐标系下的坐标。
4.定义旋转

 tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);

需要注意:
（1）setRPY()函数的参数为”turtle1”在“world”坐标系下的roll(绕X轴)，pitch(绕Y轴)，yaw(绕Z轴)
（2）为了确保转换正确强烈建议，在转换完后，运行下程序，打开rviz下使用确认下转换是否正确，我在实验中有一次，明明依次填入了三个角度，但是在rviz下发现并不正确，pitch,yaw还存在一个负号的关系（楼主对旋转角的正负号向来头晕，所以都是直接打开rviz，直接试，直观而且准确）。
5.将变换广播出去

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

其中，
（1）transform:存储变换关系的变量；
（2）ros::Time::now():广播tf使的时间戳；
（3）“world”:父坐标系的名字；
（4）turtle_name:子坐标系的名字，这里因为子坐标系有两个，所以它定义了字符串用于存放名字，方便切换，通常我们把名字填到引号里就可以了。
总结一下，假设你在机器人上应用，如果你知道机器人的位置x,y,z,与三个旋转角roll,pitch,yaw就可以广播一个tf了，如果在是平面移动机器人，则只需要知道x,y与yaw即可。

二．监听tf

通过监听tf，我们可以避免繁琐的旋转矩阵的计算，而直接获取我们需要的相关信息。
在监听中我最常用两个函数：
（1）lookupTransform（）；
（2）transformPoint（）；
（一）lookupTransform（）；函数的使用
1.函数功能：可以过得两个坐标系之间转换的关系，包括旋转与平移。
2.使用例程：http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28C++%29
3.主要步骤：
（1）定义监听器；

tf::TransformListener listener

（2）定义存放变换关系的变量

 tf::StampedTransform transform;

（3）监听两个坐标系之间的变换

   try{
      listener.lookupTransform("/turtle2", "/turtle1",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

注意：
A.由于tf的会把监听的内容存放到一个缓存中，然后再读取相关的内容，而这个过程可能会有几毫秒的延迟，也就是，tf的监听器并不能监听到“现在”的变换，所以如果不使用try,catch函数会导致报错：

“world” passed to lookupTransform argument target_frame does not exist. ”

并且会导致程序挂掉，使用try,catch之后就OK了。

B.lookupTransform()函数参数说明
a.官网的例程说的是实现从“/turtle2”到“/turtle1”的转换，但实际中使用时，我发现转换得出的坐标是在“/turtle2”坐标系下的
b.不可以把ros::Time(0)改成ros::time::now()，因为监听做不到实时，会有几毫秒的延迟。ros::Time(0)指最近时刻存储的数据，ros::time::now()则指当下，如果非要使用ros::time::now,则需要结合waitForTransform()使用，具体见:
http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28C++%29
实际中ros::Time(0)大多数情况下可以满足要求。
c.最后转换关系存入transform中

（4）使用变换
通常我们使用的是点的信息

transform.getOrigin().x()
transform.getOrigin().y(),  

可以得到在turtle1坐标系的原点，在turtle2坐标系下的位置。

transform.getRotation().getW();
transform.getRotation().getX();
transform.getRotation().getY();
transform.getRotation().getZ();

可以得到旋转的四元数，如何直接得到偏航角因为没有用到这个功能所以也就没有去找相关的函数。

（二）transformPoint（）函数使用
在实际应用中我们肯定会需要把在一个坐标系下的点转换到另一个坐标系下，这就需要transformPoint（）函数。
1.函数功能：将一个坐标系下的点的坐标转换到另一个坐标系下。
2.函数参数：如

listener_.transformPoint("PTAM_world",m_normal_pose,pose_PTAM_world)；

（1）其中m_normal_pose数据类型为 geometry_msgs::PointStamped，
其中需要定义m_normal_pose.header.frame_id即该点所属的坐标系
（2）而“PTAM_world”则指，我要将m_normal_pose转换到“PTAM_world”坐标系下。
pose_PTAM_world是转换的结果，数据类型同样为geometry_msgs::PointStamped。
3.使用例程：

geometry_msgs::PointStamped turtle1;
turtle1.header.stamp=ros::Time();
turtle1.header.frame_id="turtle1";
turtle1.point.x=1;
turtle1.point.y=2;
turtle1.point.z=3;
geometry_msgs::PointStamped turtle1_world;
        try{
listener_.transformPoint("PTAM_world",turtle1,turtle1_world);
         }
             catch (tf::TransformException &ex) {
          ROS_ERROR("%s",ex.what());
          ros::Duration(1.0).sleep();
    }

说明：同样因为延时，turtle1.header.stamp不能为ros::Time::now();否则会出类似错误

[ERROR] [1456669076.279804500]: Lookup would require extrapolation into the future. Requested time 1456669076.279616253 but the latest data is at time 1456669076.159341977, when looking up transform from frame …

总结，只有开始使用tf了才能逐渐感受到它的方便，另外再建议tf一定要与rviz结合使用，真的直观又方便。


---------------------
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
  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));
}
1.First, we pass in the transform itself.
2.Now we need to give the transform being published a timestamp, we'll just stamp it with the current time, ros::Time::now().
3.Then, we need to pass the name of the parent frame of the link we're creating, in this case "world"
4.Finally, we need to pass the name of the child frame of the link we're creating, in this case this is the name of the turtle itself. 
----
    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;
    }

If you get an error "Lookup would require extrapolation into the past" while running, you can try this alternative code to call the listener:
try {
    listener.waitForTransform(destination_frame, original_frame, ros::Time(0), ros::Duration(10.0) );
    listener.lookupTransform(destination_frame, original_frame, ros::Time(0), transform);
} catch (tf::TransformException ex) {
    ROS_ERROR("%s",ex.what());
}

其中listener.lookupTransform(destination_frame, original_frame, ros::Time(0), transform);
1.We want the transform from frame /turtle1 to frame /turtle2.
2.The time at which we want to transform. Providing ros::Time(0) will just get us the latest available transform.
3.The object in which we store the resulting transform. 
----
transformPoint（）函数使用
在实际应用中我们肯定会需要把在一个坐标系下的点转换到另一个坐标系下，这就需要transformPoint（）函数。
1.函数功能：将一个坐标系下的点的坐标转换到另一个坐标系下。
2.函数参数：如
listener_.transformPoint("PTAM_world",m_normal_pose,pose_PTAM_world)；
（1）其中m_normal_pose数据类型为 geometry_msgs::PointStamped，
其中需要定义m_normal_pose.header.frame_id即该点所属的坐标系
（2）而“PTAM_world”则指，我要将m_normal_pose转换到“PTAM_world”坐标系下。
pose_PTAM_world是转换的结果，数据类型同样为geometry_msgs::PointStamped。 


----------------------------------------------------------------------------

  /** rief Block until a transform is possible or it times out
   * param target_frame The frame into which to transform
   * param source_frame The frame from which to transform
   * param time The time at which to transform
   * param timeout How long to block before failing
   * param polling_sleep_duration How often to retest if failed
   * param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL
   */
  bool waitForTransform(const std::string& target_frame,
                        const std::string& source_frame,
                        const ros::Time& time,
                        const ros::Duration& timeout,
                        const ros::Duration& polling_sleep_duration = ros::Duration(0.01),
                        std::string* error_msg = NULL) const;

-----------------

  /** rief Get the transform between two frames by frame ID.
   * param target_frame The frame to which data should be transformed
   * param source_frame The frame where the data originated
   * param time The time at which the value of the transform is desired. (0 will get the latest)
   * param transform The transform reference to fill.  
   *
   * Possible exceptions tf::LookupException, tf::ConnectivityException,
   * tf::MaxDepthException, tf::ExtrapolationException
   */
  void lookupTransform(const std::string& target_frame, const std::string& source_frame,
                       const ros::Time& time, StampedTransform& transform) const;

---------------