ROS学习笔记（十二）：Debugging tf problems

1.Starting the example

在本教程中，我们设置了一个演示应用程序，该应用程序存在许多问题。 本教程的目的是应用系统的方法来发现这些问题。
首先，让我们运行一个示例，看看会发生什么：

  $ roslaunch turtle_tf start_debug_demo.launch

您会看到turtlesim出现。 如果从启动演示的位置选择终端窗口，则可以使用箭头键驱动其中一个机器人。 在左上角有第二个机器人。
如果演示正常运行，则第二个机器人应该跟随您可以使用箭头键命令的机器人。 显然，它不是…因为我们必须首先解决一些问题。 您看到的是以下错误消息：

[ERROR] [1593327339.332848511]: "turtle3" passed to lookupTransform argument target_frame does not exist. 
[ERROR] [1593327340.333198636]: "turtle3" passed to lookupTransform argument target_frame does not exist. 
[ERROR] [1593327341.333487791]: "turtle3" passed to lookupTransform argument target_frame does not exist. 

2.Finding the tf request

因此，如果您看一下《 tf调试问题指南》，您会发现我们首先需要弄清楚我们要求tf做什么。 因此，因此我们进入使用tf的代码部分。
在先前的教程中，我们创建了一个tf广播器，将乌龟的姿势发布到tf。 在本教程中，我们将创建一个tf侦听器以开始使用tf。

3.How to create a tf listener

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

 $ roscd learning_tf

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

   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("/turtle3", "/turtle1",
  27                                ros::Time::now(), 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 };

看一下25-28行：在这里，我们对tf进行了实际的请求。 前三个参数直接告诉我们我们要问的tf：在“现在”时间从帧/ turtle1转换为帧/ turtle3。
现在，让我们看一下对tf的请求失败的原因。

4.Checking the Frames

首先，我们想了解tf是否了解我们在/ turtle3和/ turtle1之间进行的转换：

  $ rosrun tf tf_echo turtle3 turtle1

输出告诉我们，帧turtle3不存在：

[ INFO] [1593329522.924818541]: Connected to master at [localhost:11311]
Failure at 1593329523.932194194
Exception thrown:"turtle3" passed to lookupTransform argument target_frame does not exist. 
The current list of frames is:

Failure at 1593329523.932327389
Exception thrown:"turtle3" passed to lookupTransform argument target_frame does not exist. 
The current list of frames is:

Failure at 1593329524.926591819
Exception thrown:"turtle3" passed to lookupTransform argument target_frame does not exist. 
The current list of frames is:

以上消息告诉我们确实存在哪些帧。 如果您希望获得图形化表示，请键入：

  $ rosrun tf view_frames
  $ evince frames.pdf

您将获得以下输出:
显然，问题在于我们正在请求不存在的turtle3。 要修复此错误，请在第25-28行中用turtle2替换turtle3：

   1   try{
   2     listener.lookupTransform("/turtle2", "/turtle1",
   3                              ros::Time::now(), transform);
   4   }

现在停止正在运行的演示（Ctrl-c），构建它，然后再次运行它：

  $ catkin_make
  $ roslaunch learning_tf start_debug_demo.launch

马上我们遇到了下一个问题：

[ERROR] [1593330356.675713291]: Lookup would require extrapolation into the past. 
 Requested time 1593330350.672779729 but the earliest data is at time 1593330356.193607294, 
 when looking up transform from frame [turtle1] to frame [world]

5.Checking the Timestamp

现在我们已经解决了帧名称问题，现在该看看时间戳了。 请记住，我们正在尝试在“现在”的时间在turtle2和turtle1之间进行转换。 要获取有关计时的统计信息，请运行：

 $ rosrun tf tf_monitor turtle2 turtle1

结果应如下所示：

RESULTS: for turtle2 to turtle1
Chain is: turtle2 -> world -> turtle1
Net delay     avg = 10.19: max = 11.1933

Frames:

All Broadcasters:
Node: unknown_publisher 134.985 Hz, Average Delay: 0.000383731 Max Delay: 0.00183971

这里的关键部分是从turtle2到turtle1的链延迟。 输出显示平均延迟为10秒。 这意味着tf只能在经过10秒后在海龟之间转换。 因此，如果我们要在10秒前向tf询问海龟之间的转换，而不是“现在”，tf有时会给我们答案。 让我们通过将第25-28行更改为以下内容来快速测试一下：
因此，在新代码中，我们要求在100毫秒前在海龟之间进行转换（为什么不8？为了安全起见…）。 停止演示（Ctrl-c），构建并运行：
在catkin工作区的顶部文件夹中重新生成软件包：

  $ catkin_make

然后再次运行示例

  $ roslaunch learning_tf start_debug_demo.launch

您最终应该看到乌龟移动了！

我们所做的最后一个修复并不是您真正想做的，只是为了确保这是我们的问题。 真正的解决方案如下所示：

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

or

   1   try{
   2     ros::Time now = ros::Time::now();
   3     listener.waitForTransform("/turtle2", "/turtle1", 
   4                               now, ros::Duration(1.0));
   5     listener.lookupTransform("/turtle2", "/turtle1",
   6                              now, transform);
   7   }