调试tf2问题 [待校准@8396]

Goal目标: Learn了解如何使用系统方法调试tf2相关问题。 [待校准@8397]

Tutorial教程级别: Advanced高级 [待校准@7643]

时间: 10分钟 [Alyssa@7452]

内容

背景
调试示例 [待校准@8399]
总结

背景 

本教程将引导您完成调试典型tf2问题的步骤。它还将使用许多tf2调试工具，如 tf2_echo 、 tf2_monitor 和 view_frames 。本教程假设您已经完成了 learning tf2 教程。 [待校准@8398]

调试示例 [待校准@8399]

1设置并开始示例 [待校准@8400]

在本教程中，我们将设置一个有许多问题的例程应用程序。本教程的目标是应用系统的方法来发现和解决这些问题。首先，让我们创建源文件。 [待校准@8401]

转到我们在 tf2 tutorials. Inside the ` src 目录中创建的 learning_tf2_cpp 包，复制源文件 turtle_tf2_listener.cpp ，并将其重命名为 turtle_tf2_listener_debug.cpp 。 [待校准@8402]

使用您喜欢的文本编辑器打开文件，并将第67行从 [待校准@8403]

std::string to_frame_rel = "turtle2";

到 [待校准@8404]

std::string to_frame_rel = "turtle3";

并将第75-79行中的 lookupTransform() 调用从 [待校准@8405]

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

到 [待校准@8404]

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

并保存对文件的更改。为了运行此例程，我们需要创建launch文件 start_tf2_debug_demo.launch.py 在 launch 子目录包装 learning_tf2_cpp : [待校准@8406]

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 中并构建包。 [待校准@8407]

现在让我们运行它，看看会发生什么: [待校准@8408]

ros2 launch learning_tf2_cpp start_tf2_debug_demo.launch.py

你现在会看到turtlesim出现了。同时，如果你运行 turtle_teleop_key 在另一个终端窗口，可以使用箭头键来驱动 turtle1 周围。 [待校准@8409]

ros2 run turtlesim turtle_teleop_key

你也会注意到在左下角有第二只乌龟。如果例程能够正常工作，那么第二只乌龟应该跟随你可以用箭头键命令的乌龟。然而，情况并非如此，因为我们必须首先解决一些问题。您应注意以下消息: [待校准@8410]

[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请求 [待校准@8411]

首先，我们需要找出我们到底要求tf2做什么。因此，我们进入使用tf2的代码部分。打开 src/turtle_tf2_listener_debug.cpp 文件，看看第67行: [待校准@8412]

std::string to_frame_rel = "turtle3";

第75-79行: [待校准@8413]

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

在这里，我们对tf2进行实际请求。这三个论点直接告诉我们我们在问tf2什么: 在时间 now 时从帧 turtle3 转换到帧 turtle1 。 [待校准@8414]

现在，让我们看看为什么对tf2的请求失败。 [待校准@8415]

3检查帧 [待校准@8416]

首先，为了找出tf2是否知道我们在 turtle3 和 turtle1 之间的转换，我们将使用 tf2_echo 工具。 [待校准@8417]

ros2 run tf2_ros tf2_echo turtle3 turtle1

输出告诉我们帧 turtle3 不存在: [待校准@8418]

[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 工具。 [待校准@8419]

ros2 run tf2_tools view_frames

打开生成的 frames.pdf 文件，查看以下输出: [待校准@8420]

所以很明显，问题是我们要求从帧 turtle3 转换，这是不存在的。要修复这个错误，只需在第67行用 turtle2 替换 turtle3 。 [待校准@8421]

现在停止正在运行的例程，构建它，然后再次运行它: [待校准@8422]

ros2 launch turtle_tf2 start_debug_demo.launch.py

我们马上遇到了下一个问题: [待校准@8423]

[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检查时间戳 [待校准@8424]

既然我们解决了帧名称问题，现在是时候查看时间戳了。请记住，我们正试图得到当前 turtle2 和 turtle1 之间的转变 (即 now )。要获取有关时间的统计信息，请使用相应的帧s调用 tf2_monitor 。 [待校准@8425]

ros2 run tf2_ros tf2_monitor turtle2 turtle1

结果应该如下所示: [待校准@8426]

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毫秒后在海龟之间转换。所以，如果我们在3毫秒前要求tf2在海龟之间转换，而不是 now ，tf2有时会给我们一个答案。让我们将第75-79行更改为: [待校准@8427]

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

在新代码中，我们要求在100毫秒前在海龟之间进行转换。通常使用更长的周期，只是为了确保转换将到达。停止例程，构建并运行: [待校准@8428]

ros2 launch turtle_tf2 start_debug_demo.launch.py

你应该终于看到乌龟动了! [待校准@8429]

我们做的最后一个解决方案并不是你真正想做的，只是为了确保那是我们的问题。真正的修复如下所示: [待校准@8430]

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

或者像这样: [待校准@8431]

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

您可以在 Learning about tf2 and time 教程中了解更多关于超时的信息，并如下所示使用它们: [待校准@8432]

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问题。 [待校准@8433]