三 tfTutorialsWriting a tf listener (C++)

最新推荐文章于 2022-04-30 10:32:27 发布

llz56

最新推荐文章于 2022-04-30 10:32:27 发布

阅读量216

点赞数

原文链接：http://wiki.ros.org/tf/Tutorials/Writing a tf listener (C%2B%2B)

版权

原文地址：http://wiki.ros.org/tf/Tutorials/Writing a tf listener (C%2B%2B)

Writing a tf listener (C++)

描述：本教程教您如何使用tf访问坐标系transformations。
在前面的教程中，我们创建了一个tf广播器来发布乌龟对tf的姿态。在本教程中，我们将创建一个tf侦听器来开始使用tf。

1 How to create a tf listener

让我们首先创建源文件。转到在上一教程中创建的包：

$ roscd learning_tf

1.1 The Code

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



   1 #include <ros/ros.h>
   2 #include <tf/transform_listener.h>
   3 #include <turtlesim/Velocity.h>
   4 #include <turtlesim/Spawn.h>
   5 
   6 int main(int argc, char** argv){
   7   ros::init(argc, argv, "my_tf_listener");
   8 
   9   ros::NodeHandle node;
  10 
  11   ros::service::waitForService("spawn");
  12   ros::ServiceClient add_turtle = 
  13     node.serviceClient<turtlesim::Spawn>("spawn");
  14   turtlesim::Spawn srv;
  15   add_turtle.call(srv);
  16 
  17   ros::Publisher turtle_vel = 
  18     node.advertise<turtlesim::Velocity>("turtle2/command_velocity", 10);
  19 
  20   tf::TransformListener listener;
  21 
  22   ros::Rate rate(10.0);
  23   while (node.ok()){
  24     tf::StampedTransform transform;
  25     try{
  26       listener.lookupTransform("/turtle2", "/turtle1",  
  27                                ros::Time(0), transform);
  28     }
  29     catch (tf::TransformException ex){
  30       ROS_ERROR("%s",ex.what());
  31       ros::Duration(1.0).sleep();
  32     }
  33 
  34     turtlesim::Velocity vel_msg;
  35     vel_msg.angular = 4.0 * atan2(transform.getOrigin().y(),
  36                                 transform.getOrigin().x());
  37     vel_msg.linear = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
  38                                 pow(transform.getOrigin().y(), 2));
  39     turtle_vel.publish(vel_msg);
  40 
  41     rate.sleep();
  42   }
  43   return 0;
  44 };

需要注意的是，上面的代码对于完成tf教程是必不可少的，并且它不会在ROS Hydro上编译，因为某些消息和主题的命名稍有改变。turtlesim/Velocity.h头不再使用，它已经被geometry_msgs/Twist.h代替。此外，话题/turtle/command_velovity现在称为/turtle/cmd_vel。有鉴于此，需要进行一些改变以使其工作：



   1 #include <ros/ros.h>
   2 #include <tf/transform_listener.h>
   3 #include <geometry_msgs/Twist.h>
   4 #include <turtlesim/Spawn.h>
   5 
   6 int main(int argc, char** argv){
   7   ros::init(argc, argv, "my_tf_listener");
   8 
   9   ros::NodeHandle node;
  10 
  11   ros::service::waitForService("spawn");
  12   ros::ServiceClient add_turtle =
  13     node.serviceClient<turtlesim::Spawn>("spawn");
  14   turtlesim::Spawn srv;
  15   add_turtle.call(srv);
  16 
  17   ros::Publisher turtle_vel =
  18     node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
  19 
  20   tf::TransformListener listener;
  21 
  22   ros::Rate rate(10.0);
  23   while (node.ok()){
  24     tf::StampedTransform transform;
  25     try{
  26       listener.lookupTransform("/turtle2", "/turtle1",
  27                                ros::Time(0), transform);
  28     }
  29     catch (tf::TransformException &ex) {
  30       ROS_ERROR("%s",ex.what());
  31       ros::Duration(1.0).sleep();
  32       continue;
  33     }
  34 
  35     geometry_msgs::Twist vel_msg;
  36     vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
  37                                     transform.getOrigin().x());
  38     vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
  39                                   pow(transform.getOrigin().y(), 2));
  40     turtle_vel.publish(vel_msg);
  41 
  42     rate.sleep();
  43   }
  44   return 0;
  45 };

如果在运行时出现错误“Lookup would require extrapolation into the past"，则可以尝试使用此替代代码调用侦听器：

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 The Code Explained

现在，让我们看一下与向tf发布turtle pose相关的代码。

   2 #include <tf/transform_listener.h>
   3

tf包提供了TransformListener的实现，以帮助简化接收转换的任务。要使用TransformListener，我们需要包括tf/transform_listener.h头文件。

  20   tf::TransformListener listener;

这里，我们创建一个TransformListener object。一旦侦听器被创建，它就开始通过连线接收tf transform，并将它们缓冲多达10秒。TransformListener object的作用域应该保持不变，否则它的缓存将无法填充，几乎每个查询都将失败。一种常见的方法是使TransformListener object成为类的成员变量。

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

在这里，实际工作已经完成，我们向侦听器查询特定的转换。让我们来看看四个arguments：
(1)我们需要从frame/turtle1到frame/turtle2的transform。
(2)我们想要transform的时间。提供ros::Time(0)只会让我们获得最新的可用transform。
(3)The object in which we store the resulting transform.
所有这些都被包装在一个try-catch块中，以捕获可能的异常。

  35     geometry_msgs::Twist vel_msg;
  36     vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
  37                                     transform.getOrigin().x());
  38     vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +

这里，该transform用于根据turtle2与turtle1的距离和角度计算新的线性和角速度。新的速度f发布在“turtle2/cmd_vel”话题中，sim将使用它来更新turtle2的移动。

2 Running the listener

现在我们已经创建了代码，让我们首先编译它。打开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>
    ...
    <node pkg="learning_tf" type="turtle_tf_listener"
          name="listener" />
  </launch>

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

 $ roslaunch learning_tf start_demo.launch

You should see the turtlesim with two turtles.

3 Checking the results

要查看是否有效，只需使用箭头键驱动第一只turtle（确保您的终端窗口是活动的，而不是您的模拟器窗口），您将看到第二只turtle跟在第一只turtle后面！
当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.

这是因为我们的听众试图在收到关于turtle2的消息之前计算transform，because it takes a little time to spawn in turtlesim and start broadcasting a tf frame.
现在你已经准备好进入下一个教程，在那里你将学习如何添加一个坐标系（Python）（C++）。