TF学习笔记3（建立一个监听者）

在之前的教程中，我们创建了一个tf发布者，用于将龟的姿势发布到tf。

在本教程中，我们将创建一个tf监听器以开始使用tf。

1 如何创建一个tf监听者

我们先创建源文件。

转到我们在上一个教程中创建的包：

$ roscd learning_tf

1.1 程序

启动您喜欢的编辑器并将以下代码粘贴到名为src / turtle_tf_listener.cpp的新文件中。

https://raw.github.com/ros/geometry_tutorials/groovy-devel/turtle_tf/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;
};

注意上面的代码对于完成tf教程是必不可少的，但由于某些消息和主题的命名稍有变化，它不会在ROS Hydro上编译。

所述turtlesim / Velocity.h头不再使用，它已取代geometry_msgs / Twist.h。

此外，主题/ turtle / command_velocity现在称为/ turtle / cmd_vel。鉴于此，需要进行一些更改才能使其工作：

https://raw.github.com/ros/geometry_tutorials/hydro-devel/turtle_tf/src/turtle_tf_listener.cpp

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

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

如果在运行时遇到“"Lookup would require extrapolation into the past"”的错误，您可以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());
}

1.2 程序解释

现在，让我们来看看与将turtle 位姿发布到tf相关的代码。

#include <tf/transform_listener.h>

tf包提供了TransformListener的实现，以帮助简化接收转换的任务。

要使用TransformListener，我们需要包含tf / transform_listener.h头文件。

  tf::TransformListener listener;

在这里，我们创建一个TransformListener对象

一旦创建了监听器，它就开始接收坐标转换信息，它们缓冲最多10秒钟。

The TransformListener object should be scoped to persist otherwise it's cache will be unable to fill and almost every query will fail.一种常见的方法是使TransformListener对象成为类的成员变量。

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

在这里，真正的工作已经完成，我们向侦听器查询特定的转换。我们来看看这四个参数：

我们想要从frame / turtle1到frame / turtle2的转换。
我们想要转变的时间。提供ros :: Time（0）将为我们提供最新的变换。
我们存储结果转换的对象。

补充函数：

可能的异常讲被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) +

这里，基于它与turtle1的距离和角度，变换用于计算turtle2的新线性和角速度。

新的速度发布在主题“turtle2 / cmd_vel”中，sim将使用它来更新turtle2的移动。

2 运行监听器

现在我们创建了代码，让我们先编译它。打开CMakeLists.txt文件，并在底部添加以下行，否则编译失败：

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

在catkin工作区的顶层文件夹中构建包：

  $ catkin_make

如果一切顺利，您应该在devel / lib / learning_tf文件夹中有一个名为turtle_tf_listener的二进制文件。

如果是这样，我们就可以为它添加启动文件了。使用文本编辑器，打开名为start_demo.launch的启动文件，并在<launch>块内合并下面的节点：

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

首先，确保您停止了上一个教程中的启动文件（使用ctrl-c）。现在您已准备好开始您的全龟演示：

 $ roslaunch learning_tf start_demo.launch

你应该看到有两只乌龟的乌龟。

3 检查结果

要查看是否有效，只需使用箭头键驱动第一只乌龟（确保终端窗口处于活动状态，而不是模拟器窗口），然后您将看到第二只乌龟跟随第一只乌龟！

当turtlesim启动时你可能会看到：

[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.

发生这种情况是因为我们的侦听器在收到有关乌龟2的消息之前尝试计算变换，因为它需要一点时间在turtlesim中生成并开始广播tf坐标。

现在，您已准备好继续学习下一个教程，在那里您将学习如何添加框架（Python）（C ++）

放个图片：