Skip to content

canTransform reports misleading extrapolation time when multiple parent frames require extrapolation #832

@polortiz4

Description

@polortiz4

Generated by Generative AI

an internal tool was used to help generate this ticket, but I can't disclose it

Operating System:

24.04.1-Ubuntu SMP PREEMPT_DYNAMIC Thu Aug 14 16:52:50 UTC 2 x86_64 x86_64 x86_64 GNU/Linux

ROS version or commit hash:

humble

RMW implementation (if applicable):

No response

RMW Configuration (if applicable):

No response

Client library (if applicable):

rclcpp

'ros2 doctor --report' output

Bug Description

When querying a transform where both the target frame and its parent have stale connections to their respective parents, canTransform reports the latest time based on the target disconnection rather than the most relevant one for the requested transform.

Steps to reproduce issue

Create a workspace with

# CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(tf2_bug_demo_ros2)

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)

add_executable(tf2_bug_repro_ros2 tf2_bug_repro_ros2.cpp)
ament_target_dependencies(tf2_bug_repro_ros2 rclcpp tf2_ros geometry_msgs)

install(TARGETS tf2_bug_repro_ros2 DESTINATION lib/${PROJECT_NAME})

ament_package()

  <!-- package.xml -->

<?xml version="1.0"?>
<package format="3">
  <name>tf2_bug_demo_ros2</name>
  <version>0.0.1</version>
  <description>ROS2 reproduction of tf2_ros canTransform error reporting bug</description>
  <maintainer email="[email protected]">Test</maintainer>
  <license>MIT</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>tf2_ros</depend>
  <depend>geometry_msgs</depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

// tf2_bug_repro_ros2.cpp

#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/transform_stamped.hpp>

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("tf2_bug_demo_ros2");
    
    tf2_ros::TransformBroadcaster br(node);
    tf2_ros::Buffer buffer(node->get_clock());
    tf2_ros::TransformListener listener(buffer);
    
    auto now = node->get_clock()->now();
    auto very_old = now - rclcpp::Duration::from_seconds(10.0);    // map->odom stale by 10s
    auto slightly_old = now - rclcpp::Duration::from_seconds(0.1); // odom->base_link stale by 100ms
    
    // Publish 2 stale map->odom transforms
    for (int i = 0; i < 2; i++) {
        geometry_msgs::msg::TransformStamped map_odom;
        map_odom.header.stamp = rclcpp::Time(very_old.nanoseconds() + i * 10000000); // 10ms offset
        map_odom.header.frame_id = "map";
        map_odom.child_frame_id = "odom";
        map_odom.transform.rotation.w = 1.0;
        br.sendTransform(map_odom);
    }
    
    // Publish 2 slightly stale odom->base_link transforms  
    for (int i = 0; i < 2; i++) {
        geometry_msgs::msg::TransformStamped odom_base;
        odom_base.header.stamp = rclcpp::Time(slightly_old.nanoseconds() + i * 10000000); // 10ms offset
        odom_base.header.frame_id = "odom";
        odom_base.child_frame_id = "base_link";
        odom_base.transform.rotation.w = 1.0;
        br.sendTransform(odom_base);
    }
    
    rclcpp::sleep_for(std::chrono::milliseconds(500));
    
    // Query odom->base_link at current time (should fail due to 100ms staleness)
    // But error will report the 10s staleness from map->odom instead
    std::string error_msg;
    bool can_transform = buffer.canTransform("odom", "base_link", now, 
                                           tf2::durationFromSec(0.0), &error_msg);
    
    if (!can_transform) {
        RCLCPP_ERROR(node->get_logger(), "BUG: Error reports %s", error_msg.c_str());
    }
    
    rclcpp::shutdown();
    return 0;
}

Expected behavior

An log line mentioning the 100ms old timestamp as the latest data time.

[ERROR] [1758300905.976464776] [tf2_bug_demo_ros2]: BUG: Error reports Lookup would require extrapolation into the future.  Requested time 1758300905.476031 but the latest data is at time 1758300905.386031, when looking up transform from frame [odom] to frame [base_link]. canTransform returned after 9.766e-06 timeout was 0.

Actual behavior

A log line mentioning the 10s old timestamp as the latest data time. Even though map->odom is an irrelevant connection when it comes to odom->base_link

[ERROR] [1758300905.976251161] [tf2_bug_demo_ros2]: BUG: Error reports Lookup would require extrapolation into the future.  Requested time 1758300905.476031 but the latest data is at time 1758300895.486031, when looking up transform from frame [base_link] to frame [odom]. canTransform returned after 6.3774e-05 timeout was 0.

Additional information

Impact

This makes debugging transform issues confusing, as users get error messages about unrelated transform failures (like map->odom) rather than the actual problem they need to fix (odom->base_link staleness).

This is particularly confusing since the lookup odom->base_link and the lookup base_link->odom report different latest_data times, which is counterintuitive.

Environment

  • ROS Version: ROS2 Humble
  • tf2_ros Version: [as included in ROS2 Humble]

Cross-Platform Issue

This same bug has been verified to exist in both ROS1 (Kinetic) and ROS2 (Humble).

Metadata

Metadata

Assignees

No one assigned

    Labels

    bugSomething isn't workinghelp wantedExtra attention is needed

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions