调试

**目标：**学习如何使用系统化方法来调试与tf2相关的问题。

教程级别： 中级

时间: 10分钟

内容

背景
调试示例
总结

背景 

本教程将引导您逐步调试一个典型的tf2问题。它还将使用许多tf2调试工具，如 tf2_echo、tf2_monitor 和 view_frames。本教程假设您已经完成了学习tf2 教程。

调试示例 

1 设置并启动示例 

在本教程中，我们将设置一个演示应用程序，该应用程序存在一些问题。本教程的目标是应用系统化的方法来发现和解决这些问题。首先，让我们创建源文件。

进入我们在 tf2 tutorials 中创建的 learning_tf2_cpp 包。在 src 目录中复制源文件 turtle_tf2_listener.cpp，并将其重命名为 turtle_tf2_listener_debug.cpp。

使用您首选的文本编辑器打开该文件，并将第67行更改为

std::string toFrameRel = "turtle2";

更改为

std::string toFrameRel = "turtle3";

并将第75-79行中的 lookupTransform() 调用更改为

try {
   transformStamped = tf_buffer_->lookupTransform(
     toFrameRel,
     fromFrameRel,
     tf2::TimePointZero);
} catch (tf2::TransformException & ex) {

更改为

try {
   transformStamped = tf_buffer_->lookupTransform(
     toFrameRel,
     fromFrameRel,
     this->now());
} catch (tf2::TransformException & ex) {

并保存文件更改。为了运行这个演示，我们需要在 learning_tf2_cpp 包的 launch 子目录中创建一个名为 start_tf2_debug_demo.launch.py 的启动文件：

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node

def generate_launch_description():
   return LaunchDescription([
      DeclareLaunchArgument(
         'target_frame', default_value='turtle1',
         description='Target frame name.'
      ),
      Node(
         package='turtlesim',
         executable='turtlesim_node',
         name='sim',
         output='screen'
      ),
      Node(
         package='learning_tf2_cpp',
         executable='turtle_tf2_broadcaster',
         name='broadcaster1',
         parameters=[
               {'turtlename': 'turtle1'}
         ]
      ),
      Node(
         package='learning_tf2_cpp',
         executable='turtle_tf2_broadcaster',
         name='broadcaster2',
         parameters=[
               {'turtlename': 'turtle2'}
         ]
      ),
      Node(
         package='learning_tf2_cpp',
         executable='turtle_tf2_listener_debug',
         name='listener_debug',
         parameters=[
               {'target_frame': LaunchConfiguration('target_frame')}
         ]
      ),
   ])

不要忘记将``turtle_tf2_listener_debug``可执行文件添加到``CMakeLists.txt``中并构建软件包。

现在让我们运行它看看会发生什么：

ros2 launch learning_tf2_cpp start_tf2_debug_demo.launch.py

现在你会看到turtlesim启动了。同时，如果你在另一个终端窗口中运行``turtle_teleop_key``，你可以使用箭头键来控制``turtle1``移动。

ros2 run turtlesim turtle_teleop_key

你还会注意到在左下角有一只第二只海龟。如果演示工作正常，这只第二只海龟应该会跟随你用箭头键控制的海龟移动。然而，事实并非如此，因为我们首先需要解决一些问题。你应该注意到以下消息：

[turtle_tf2_listener_debug-4] [INFO] [1630223454.942322623] [listener_debug]: Could not
transform turtle3 to turtle1: "turtle3" passed to lookupTransform argument target_frame
does not exist

2 寻找tf2请求 

首先，我们需要弄清楚我们具体要求tf2做什么。因此，我们进入正在使用tf2的代码部分。打开``src/turtle_tf2_listener_debug.cpp``文件，并查看第67行：

std::string to_frame_rel = "turtle3";

并且在第75至79行中：

try {
   transformStamped = tf_buffer_->lookupTransform(
     toFrameRel,
     fromFrameRel,
     this->now());
} catch (tf2::TransformException & ex) {

在这里，我们实际向tf2发出请求。这三个参数直接告诉我们向tf2询问的内容：在时间``now``下从坐标系``turtle3``到坐标系``turtle1``的变换。

现在，让我们看看为什么这个向tf2的请求失败了。

3 检查坐标系 

首先，为了确定tf2是否知道我们在``turtle3``和``turtle1``之间的变换，我们将使用``tf2_echo``工具。

ros2 run tf2_ros tf2_echo turtle3 turtle1

输出告诉我们``turtle3``坐标系不存在：

[INFO] [1630223557.477636052] [tf2_echo]: Waiting for transform turtle3 ->  turtle1:
Invalid frame ID "turtle3" passed to canTransform argument target_frame - frame does
not exist

那么存在哪些帧？如果您想以图形方式查看，请使用“view_frames”工具。

ros2 run tf2_tools view_frames

打开生成的“frames.pdf”文件，查看以下输出：

显然问题是我们正在请求从“turtle3”帧到“turtle2”帧的变换，但“turtle3”帧不存在。要修复此错误，请在第67行将“turtle3”替换为“turtle2”。

现在停止正在运行的演示程序，进行构建，然后再次运行：

ros2 launch turtle_tf2 start_debug_demo.launch.py

然后立即遇到下一个问题：

[turtle_tf2_listener_debug-4] [INFO] [1630223704.617382464] [listener_debug]: Could not
transform turtle2 to turtle1: Lookup would require extrapolation into the future. Requested
time 1630223704.617054 but the latest data is at time 1630223704.616726, when looking up
transform from frame [turtle1] to frame [turtle2]

4 检查时间戳 

现在我们已经解决了帧名称的问题，是时候看一下时间戳了。记住，我们要在当前时间（即“now”）获取“turtle2”和“turtle1”之间的变换。要获取有关时间统计的信息，请调用相应帧的“tf2_monitor”。

ros2 run tf2_ros tf2_monitor turtle2 turtle1

结果应该是这样的：

RESULTS: for turtle2 to turtle1
Chain is: turtle1
Net delay     avg = 0.00287347: max = 0.0167241

Frames:
Frame: turtle1, published by <no authority available>, Average Delay: 0.000295833, Max Delay: 0.000755072

All Broadcasters:
Node: <no authority available> 125.246 Hz, Average Delay: 0.000290237 Max Delay: 0.000786781

这里的关键部分是从“turtle2”到“turtle1”的延迟。输出显示平均延迟约为3毫秒。这意味着tf2只能在经过3毫秒后才能在这些海龟之间进行变换。因此，如果我们询问tf2在3毫秒前而不是“now”时的变换，有时tf2能给出答案。让我们通过将75-79行更改为以下内容来快速测试：

try {
   transformStamped = tf_buffer_->lookupTransform(
     toFrameRel,
     fromFrameRel,
     this->now() - rclcpp::Duration::from_seconds(0.1));
} catch (tf2::TransformException & ex) {

在新代码中，我们要求在100毫秒之前的时间获取这些海龟之间的变换。通常使用更长的时间段，只是为了确保变换会到达。停止演示，构建和运行：

ros2 launch turtle_tf2 start_debug_demo.launch.py

你应该最终看到海龟在移动！

我们刚才进行的最后修复不是你想要做的，只是为了确保这是我们的问题。真正的修复将如下所示：

try {
   transformStamped = tf_buffer_->lookupTransform(
     toFrameRel,
     fromFrameRel,
     tf2::TimePointZero);
} catch (tf2::TransformException & ex) {

或者像这样：

try {
   transformStamped = tf_buffer_->lookupTransform(
     toFrameRel,
     fromFrameRel,
     tf2::TimePoint());
} catch (tf2::TransformException & ex) {

你可以在:doc:`使用时间<./Learning-About-Tf2-And-Time-Cpp>`教程中了解更多关于超时的信息，并按照以下方式使用它们：

try {
   transformStamped = tf_buffer_->lookupTransform(
     toFrameRel,
     fromFrameRel,
     this->now(),
     rclcpp::Duration::from_seconds(0.05));
} catch (tf2::TransformException & ex) {

总结 

在本教程中，您学习了如何使用系统性的方法来调试与tf2相关的问题。您还学习了如何使用tf2调试工具，例如``tf2_echo``、``tf2_monitor``和``view_frames``来帮助您调试这些tf2问题。

调试

背景

调试示例

1 设置并启动示例

2 寻找tf2请求

3 检查坐标系

4 检查时间戳

总结