diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 98d4a108f..eadf44314 100644 --- a/tf2/src/buffer_core.cpp +++ b/tf2/src/buffer_core.cpp @@ -355,6 +355,7 @@ tf2::TF2Error BufferCore::walkToTopParent( std::string extrapolation_error_string; bool extrapolation_might_have_occurred = false; + TimePoint extrapolation_latest_time = TimePointZero; while (frame != 0) { TimeCacheInterfacePtr cache = getFrame(frame); @@ -373,6 +374,7 @@ tf2::TF2Error BufferCore::walkToTopParent( // Just break out here... there may still be a path from source -> target top_parent = frame; extrapolation_might_have_occurred = true; + extrapolation_latest_time = cache->getLatestTimestamp(); break; } @@ -416,6 +418,25 @@ tf2::TF2Error BufferCore::walkToTopParent( CompactFrameID parent = f.gather(cache, time, error_string); if (parent == 0) { + if (extrapolation_might_have_occurred) { + // Shouldn't treat second walk path failure as extrapolation if the + // first walk path failure is older than the latest data in the cache + TimePoint phase2_latest = cache->getLatestTimestamp(); + + // prefer source for tie-breaker + bool prefer_phase1 = (extrapolation_latest_time >= phase2_latest); + + if (prefer_phase1) { + if (error_string) { + std::stringstream ss; + ss << extrapolation_error_string << ", when looking up transform from frame [" + << lookupFrameString(source_id) << "] to frame [" << lookupFrameString(target_id) + << "]"; + *error_string = ss.str(); + } + return tf2::TF2Error::TF2_EXTRAPOLATION_ERROR; + } + } if (error_string) { std::stringstream ss; ss << *error_string << ", when looking up transform from frame [" << lookupFrameString( diff --git a/tf2/test/simple_tf2_core.cpp b/tf2/test/simple_tf2_core.cpp index 68bf6e507..ab4266c6c 100644 --- a/tf2/test/simple_tf2_core.cpp +++ b/tf2/test/simple_tf2_core.cpp @@ -338,6 +338,59 @@ TEST(tf2_time, To_From_Duration) } } +TEST(tf2_canTransform, ExtrapolationErrorReportsCorrectLatestTime) +{ + // this test verifies if correct time difference between transforms + // are reported when transform from a to b fails in both source-to-root + // and root-to-source walks. + tf2::BufferCore tfc; + geometry_msgs::msg::TransformStamped st; + std::string error_msg; + + // 1. publish map -> odom (stale by 10s, latest is 90.01) + st.header.frame_id = "map"; + st.child_frame_id = "odom"; + st.transform.rotation.w = 1.0; + + st.header.stamp.sec = 90; + st.header.stamp.nanosec = 0; + tfc.setTransform(st, "authority1"); + + st.header.stamp.sec = 90; + // 90.01s + st.header.stamp.nanosec = 10000000; + tfc.setTransform(st, "authority1"); + + // 2. publish odom -> base_link (stale by 0.1s, latest is 99.91) + st.header.frame_id = "odom"; + st.child_frame_id = "base_link"; + + st.header.stamp.sec = 99; + // 99.90s + st.header.stamp.nanosec = 900000000; + tfc.setTransform(st, "authority1"); + + st.header.stamp.sec = 99; + // 99.91s + st.header.stamp.nanosec = 910000000; + tfc.setTransform(st, "authority1"); + + // 3. query odom -> base_link at T=100.0s + bool can_transform = tfc.canTransform( + "odom", "base_link", + tf2::timeFromSec(100.0), + &error_msg); + + EXPECT_FALSE(can_transform); + + // 4. verify the error message: + // the message should report the latest data for odom->base_link (99.91), + // not map->odom (90.01). + EXPECT_NE(error_msg.find("99.91"), std::string::npos); + + EXPECT_EQ(error_msg.find("90.01"), std::string::npos); +} + TEST(tf2_convert, Covariance_RowMajor_To_Nested) { // test verifies the correct conversion of the flat covariance array to a