ROS2与tf2入门教程-调试tf2

说明：

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

调试示例

• 1 设置和启动示例
• 对于本教程，我们将设置一个有许多问题的演示应用程序。
• 本教程的目标是应用系统的方法来发现和解决这些问题。
• 首先，让我们创建源文件。
• 转到我们在 tf2 教程中创建的 learning_tf2_cpp 包。
• 在 src 目录中复制源文件 turtle_tf2_listener.cpp 并将其重命名为 turtle_tf2_listener_debug.cpp。

cd ~/tf2_ws/src/learning_tf2_cpp/src
cp turtle_tf2_listener.cpp turtle_tf2_listener_debug.cpp
vim turtle_tf2_listener_debug.cpp

• 修改以下内容

std::string to_frame_rel = "turtle3";

• 将lookupTransform()第 75-79 行中修改

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

• 修改CMakeLists.txt文件，再原来的内容基础上添加turtle_tf2_listener_debug.cpp的内容

add_executable(turtle_tf2_listener_debug src/turtle_tf2_listener_debug.cpp)
ament_target_dependencies(
  turtle_tf2_listener_debug
  geometry_msgs
  rclcpp
  tf2
  tf2_ros
  turtlesim
)
install(TARGETS
  turtle_tf2_listener_debug
  DESTINATION lib/${PROJECT_NAME})

• 创建start_tf2_debug_demo.launch.py

cd ~/tf2_ws/src/learning_tf2_cpp/launch
cp turtle_tf2_demo.launch.py start_tf2_debug_demo.launch.py
vim start_tf2_debug_demo.launch.py

• 修改内容如下

  Node(
     package='learning_tf2_cpp',
     executable='turtle_tf2_listener_debug',
     name='listener_debug',
     parameters=[
           {'target_frame': LaunchConfiguration('target_frame')}
     ]
  ),

• 构建

colcon build --symlink-install --packages-select learning_tf2_cpp

• 加载工作空间

. ~/tf2_ws/install/local_setup.bash

测试：

• 运行

. ~/tf2_ws/install/local_setup.bash
ros2 launch learning_tf2_cpp start_tf2_debug_demo.launch.py

• 键盘控制

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

• 会提示错误信息，因为我们缺少坐标系turtle3，导致变换异常

问题分析：

• 查找tf2请求。
• 进入使用 tf2 的代码部分。
• 打开src/turtle_tf2_listener_debug.cpp文件，看看第 67 行

std::string to_frame_rel = "turtle3";

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

• 这里我们对 tf2 进行实际请求。

• 这三个参数直接告诉我们我们在问什么

• 查找在当前时间从坐标系turtle3到坐标系turtle1变换

• 检查坐标系。

• 首先，要找出 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

• 打开生成evince frames.pdf文件
• 可以看到以下输出：

• 很明显，问题是我们正在turtle3从不存在的frame 请求转换。
• 要修复此错误，只需在第 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]

• 检查时间戳。
• 现在我们解决了坐标系问题，是时候查看时间戳了。
• 请记住，我们正在尝试在当前时间（即）turtle2和之间进行转换。
• 要获取有关计时的统计信息，请使用相应的坐标系调用。

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) {

• 以在了解 tf2 和时间教程中了解有关超时的更多信息，并按如下方式使用它们：

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

• 修复后，在运行就不再提示错误

参考：
　

• 资料来源