调试

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

教程级别：中级

时间：10 分钟

背景 

本教程将引导您完成调试典型 tf2 问题的步骤。它还将使用许多 tf2 调试工具，例如“tf2_echo”、“tf2_monitor”和“view_frames”。本教程假设您已完成 :doc:“learning tf2 <./Tf2-Main>”教程。

调试示例 

1 设置和启动示例 

对于本教程，我们将设置一个包含许多问题的演示应用程序。本教程的目标是应用系统方法来查找和解决这些问题。首先，让我们创建源文件。

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

使用您喜欢的文本编辑器打开文件，并将第 65 行从

std::string toFrameRel = "turtle2";

to

std::string toFrameRel = "turtle3";

并将第 73-77 行中的“lookupTransform()”调用从

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

to

try {
  t = tf_buffer_->lookupTransform(
    toFrameRel, fromFrameRel,
    this->now());
} catch (const 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”文件，查看第 65 行：

std::string to_frame_rel = "turtle3";

以及第73-77行：

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

这里我们向 tf2 发出实际请求。这三个参数直接告诉我们我们要求 tf2 做什么：在时间“现在”从帧“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”。

要修复此错误，只需在第 65 行将“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 检查时间戳 

现在我们解决了帧名称问题，是时候查看时间戳了。请记住，我们正尝试获取当前时间（即“现在”）的“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 毫秒前而不是“现在”进行海龟之间的变换，tf2 有时能够给我们一个答案。让我们通过将第 73-77 行更改为以下内容来快速测试一下：

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

在新代码中，我们要求在 100 毫秒前进行海龟之间的变换。通常使用较长的周期，只是为了确保变换能够到达。停止演示，构建并运行：

ros2 launch turtle_tf2 start_debug_demo.launch.py

最后你就能看见乌龟动了！

我们最后做的修复并不是你真正想要做的，只是为了确保这是我们的问题。真正的修复看起来是这样的：

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

或者像这样：

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

您可以在：doc:使用时间教程中了解有关超时的更多信息，并按如下所示使用它们：

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

摘要 

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

调试

背景

调试示例

1 设置和启动示例

2 查找 tf2 请求

3 检查帧

4 检查时间戳

摘要