ROS_TF:Writing a tf listener

In the previous tutorials we created a tf broadcaster to publish the pose of a turtle to tf. In this tutorial we’ll create a tf listener to start using tf.
在之前的教程中，我们创建了一个tf广播并且将乌龟的状态发布给tf。在本教程中我们将创建一个tf监听器，开始使用tf。

1.How to create a tf listener

Let’s first create the source files. Go to the package we created in the previous tutorial:
创建源文件。转到在上一个教程中创建的包。

 $ roscd learning_tf

1.1 The Code

Fire up your favorite editor and paste the following code into a new file called src/turtle_tf_listener.cpp.

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <turtlesim/Velocity.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_listener");

  ros::NodeHandle node;

  ros::service::waitForService("spawn");
  ros::ServiceClient add_turtle = 
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn srv;
  add_turtle.call(srv);

  ros::Publisher turtle_vel = 
    node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 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();
    }

    turtlesim::Velocity vel_msg;
    vel_msg.angular = 4.0 * atan2(transform.getOrigin().y(),
                                transform.getOrigin().x());
    vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                pow(transform.getOrigin().y(), 2));
    turtle_vel.publish(vel_msg);

    rate.sleep();
  }
  return 0;
};

It’s important to note that the above code is essential to finish the tf tutorials and it will NOT compile on ROS Hydro, due to a slight change in the naming of certain messages and topics. The turtlesim/Velocity.h header is not used anymore, it has been replaced by geometry_msgs/Twist.h. Furthermore, the topic /turtle/command_velocity is now called /turtle/cmd_vel. In light of this, a few changes are necessary to make it work:
需要注意的是，上述代码对于完成tf教程至关重要，并且由于某些消息和主题的命名略有变化，因此它不会再ROS Hydro上编译。鉴于以上，我们不在使用turtlesim/Velocity.h头文件，取而代之，用geometry_msgs/Twist.h取代。此外，主题/turtle/command_velocity现在称之为/turtle/cmd_vel。有鉴于此，需要做一些更改才能使其工作。

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

  ros::service::waitForService("spawn");
  ros::ServiceClient add_turtle =
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn srv;
  add_turtle.call(srv);

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

  tf::TransformListener listener;       //定义tf接收器

  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;    //定义个Stamped tf的转换关系
    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;
};

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) );    //等待需要的tf转换关系生成
    listener.lookupTransform(destination_frame, original_frame, ros::Time(0), transform);           //查阅destination_frame，转换成original_frame
} catch (tf::TransformException ex) {
    ROS_ERROR("%s",ex.what());
}

1.2 The Code Explained

Now, let’s take a look at the code that is relevant to publishing the turtle pose to tf.

#include <tf/transform_listener.h>

The tf package provides an implementation of a TransformListener to help make the task of receiving transforms easier. To use the TransformListener, we need to include the tf/transform_listener.h header file.

tf::TransformListener listener;

Here, we create a TransformListener object. Once the listener is created, it starts receiving tf transformations over the wire, and buffers them for up to 10 seconds. The TransformListener object should be scoped to persist otherwise it’s cache will be unable to fill and almost every query will fail. A common method is to make the TransformListener object a member variable of a class.
这里，我们创建一个TranformListener对象。一旦创建了监听器，它就开始通过线路接受tf变换，并将其缓冲秒。TransformListener对象的作用域应该是持久的，否则它的缓存将无法填充，几乎每个查询都将失败。一种常见的方法是使TransformListener对象成为类的成员变量。

try{
  listener.lookupTransform("/turtle2", "/turtle1",
                           ros::Time(0), transform);
}

Here, the real work is done, we query the listener for a specific transformation. Let’s take a look at the four arguments:
此处，才是真正的工作所在。我们向监听器查询特定的转换，让我们看一下四个变量。
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.

1.头两个参数代表着转换的坐标，此处，我们从turtle1向turtle2变换。
2.第三个参数代表时间点
3.第四个坐标用于存储转换结果。

All this is wrapped in a try-catch block to catch possible exceptions.
所有这些都被包装在一个try catch块中以捕获可能出现的异常情况。

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

Here, the transform is used to calculate new linear and angular velocities for turtle2, based on its distance and angle from turtle1. The new velocity is published in the topic “turtle2/cmd_vel” and the sim will use that to update turtle2’s movement.
此处，转换被用来计算乌龟2的新的线速度和角速度，基于它和乌龟1的距离和角度。新的速度发布在主题"turtle2/cmd_vel"中，sim（指的是两只乌龟所在的窗口程序)将用它来更新turtle2的运动。

2.Running the listener

Now that we created the code, lets compile it first. Open the CMakeLists.txt file, and add the following line on the bottom:

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

Build your package at the top folder of your catkin workspace:

 $ catkin_make

If everything went well, you should have a binary file called turtle_tf_listener in your devel/lib/learning_tf folder.

If so, we’re ready add it the launch file for this demo. With your text editor, open the launch file called start_demo.launch, and merge the node block below inside the block:

 <launch>
    ...
    <node pkg="learning_tf" type="turtle_tf_listener"
          name="listener" />
  </launch>

First, make sure you stopped the launch file from the previous tutorial (use ctrl-c). Now you’re ready to start your full turtle demo:

 $ roslaunch learning_tf start_demo.launch

You should see the turtlesim with two turtles.

3.Checking the results

To see if things work, simply drive around the first turtle using the arrow keys (make sure your terminal window is active, not your simulator window), and you’ll see the second turtle following the first one!

When the turtlesim starts up you may see:

[ERROR] 1253915565.300572000: Frame id /turtle2 does not exist! When trying to transform between /turtle1 and /turtle2.
[ERROR] 1253915565.401172000: Frame id /turtle2 does not exist! When trying to transform between /turtle1 and /turtle2.

This happens because our listener is trying to compute the transform before messages about turtle 2 have been received because it takes a little time to spawn in turtlesim and start broadcasting a tf frame.

Now you’re ready to move on to the next tutorial, where you’ll learn how to add a frame (Python) (C++)