Fix misleading extrapolation time in buffer_core (#832) (#896)

(cherry picked from commit 876d31e)

Author: selimrecep

tf2/src/buffer_core.cpp

@@ -359,6 +359,7 @@ tf2::TF2Error BufferCore::walkToTopParent(
   TF2Error extrapolation_error_code = TF2Error::TF2_NO_ERROR;
   std::string extrapolation_error_string;
   bool extrapolation_might_have_occurred = false;
+  TimePoint extrapolation_latest_time = TimePointZero;
 
   while (frame != 0) {
     TimeCacheInterfacePtr cache = getFrame(frame);
@@ -379,6 +380,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;
     }
 
@@ -422,6 +424,25 @@ tf2::TF2Error BufferCore::walkToTopParent(
 
     CompactFrameID parent = f.gather(cache, time, error_string, &error_code);
     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 extrapolation_error_code;
+        }
+      }
       if (error_string) {
         std::stringstream ss;
         ss << *error_string << ", when looking up transform from frame [" << lookupFrameString(

tf2/test/simple_tf2_core.cpp

@@ -386,6 +386,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