Fix issue #749

tf2/CMakeLists.txt

@@ -93,6 +93,11 @@ if(BUILD_TESTING)
     target_link_libraries(test_cache_unittest tf2)
   endif()
 
+  ament_add_gtest(test_chainAsVector test/chainAsVector.cpp)
+  if(TARGET test_chainAsVector)
+    target_link_libraries(test_chainAsVector tf2)
+  endif()
+
   ament_add_google_benchmark(cache_benchmark test/cache_benchmark.cpp)
   if(TARGET cache_benchmark)
     target_link_libraries(cache_benchmark tf2)

tf2/src/buffer_core.cpp

@@ -479,14 +479,17 @@ tf2::TF2Error BufferCore::walkToTopParent(
     }
     // Erase all duplicate items from frame_chain
     if (n > 0u) {
-      frame_chain->erase(frame_chain->begin() + (n - 1u), frame_chain->end());
+      frame_chain->erase(frame_chain->begin() + (n + 1u), frame_chain->end());
     }
 
     if (m < reverse_frame_chain.size()) {
-      size_t i = m + 1uL;
-      while (i > 0u) {
-        --i;
+      size_t i = m + 1;
+      while (true) {
         frame_chain->push_back(reverse_frame_chain[i]);
+        if(i == 0u) {
+          break;
+        }
+        --i;
       }
     }
   }
@@ -1583,44 +1586,46 @@ void BufferCore::_chainAsVector(
     }
   }
 
-  if (source_time != target_time) {
-    std::vector<CompactFrameID> target_frame_chain;
-    retval = walkToTopParent(
-      accum, target_time, target_id, fixed_id, &error_string,
-      &target_frame_chain);
+  std::vector<CompactFrameID> target_frame_chain;
+  retval = walkToTopParent(
+    accum, target_time, fixed_id, target_id, &error_string,
+    &target_frame_chain);
 
-    if (retval != tf2::TF2Error::TF2_NO_ERROR) {
-      switch (retval) {
-        case tf2::TF2Error::TF2_CONNECTIVITY_ERROR:
-          throw ConnectivityException(error_string);
-        case tf2::TF2Error::TF2_EXTRAPOLATION_ERROR:
-          throw ExtrapolationException(error_string);
-        case tf2::TF2Error::TF2_LOOKUP_ERROR:
-          throw LookupException(error_string);
-        default:
-          CONSOLE_BRIDGE_logError("Unknown error code: %d", retval);
-          assert(0);
-      }
-    }
-    size_t m = target_frame_chain.size();
-    size_t n = source_frame_chain.size();
-    while (m > 0u && n > 0u) {
-      --m;
-      --n;
-      if (source_frame_chain[n] != target_frame_chain[m]) {
-        break;
-      }
+  if (retval != tf2::TF2Error::TF2_NO_ERROR) {
+    switch (retval) {
+      case tf2::TF2Error::TF2_CONNECTIVITY_ERROR:
+        throw ConnectivityException(error_string);
+      case tf2::TF2Error::TF2_EXTRAPOLATION_ERROR:
+        throw ExtrapolationException(error_string);
+      case tf2::TF2Error::TF2_LOOKUP_ERROR:
+        throw LookupException(error_string);
+      default:
+        CONSOLE_BRIDGE_logError("Unknown error code: %d", retval);
+        assert(0);
     }
-    // Erase all duplicate items from frame_chain
-    if (n > 0u) {
-      source_frame_chain.erase(source_frame_chain.begin() + (n - 1u), source_frame_chain.end());
+  } 
+
+  size_t m = target_frame_chain.size();
+  size_t n = source_frame_chain.size();
+  while (m > 0u && n > 0u) {
+    --m;
+    --n;
+    if (source_frame_chain[n] != target_frame_chain[m]) {
+      break;
     }
+  }
+  // Erase all duplicate items from frame_chain
+  if (n > 0u) {
+    source_frame_chain.erase(source_frame_chain.begin() + (n + 1u), source_frame_chain.end());
+  }
 
-    if (m < target_frame_chain.size()) {
-      for (size_t i = 0u; i <= m; ++i) {
-        source_frame_chain.push_back(target_frame_chain[i]);
-      }
+  size_t i = m + 1;
+  while (true) {
+    source_frame_chain.push_back(target_frame_chain[i]);
+    if(i == 0u) {
+      break;
     }
+    --i;
   }
 
   // Write each element of source_frame_chain as string

tf2/test/chainAsVector.cpp

@@ -0,0 +1,144 @@
+// Copyright 2023, Your Name. All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the Willow Garage nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+#include <gtest/gtest.h>
+
+#include <chrono>
+#include <string>
+#include <vector>
+
+#include "builtin_interfaces/msg/time.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+
+#include "tf2/buffer_core.hpp"
+#include "tf2/exceptions.hpp"
+#include "tf2/time.hpp"
+
+TEST(tf2_chainAsVector, ValidChain)
+{
+  tf2::BufferCore buffer;
+
+  // Populate buffer with transforms: C -> B -> A (fixed) and D -> A
+  geometry_msgs::msg::TransformStamped transform;
+
+  // A -> B
+  transform.header.stamp.sec = 1;
+  transform.header.frame_id = "A";
+  transform.child_frame_id = "B";
+  transform.transform.rotation.w = 1.0;
+  ASSERT_TRUE(buffer.setTransform(transform, "test"));
+
+  // B -> C
+  transform.header.frame_id = "B";
+  transform.child_frame_id = "C";
+  ASSERT_TRUE(buffer.setTransform(transform, "test"));
+
+  // A -> D
+  transform.header.frame_id = "A";
+  transform.child_frame_id = "D";
+  ASSERT_TRUE(buffer.setTransform(transform, "test"));
+
+  // Call _chainAsVector for source=C, target=D, fixed=A
+  std::vector<std::string> output;
+  buffer._chainAsVector(
+      "D", tf2::timeFromSec(1.0), // target
+      "C", tf2::timeFromSec(1.0), // source
+      "A",                        // fixed
+      output);
+
+  // Verify the resulting chain
+  std::vector<std::string> expected{"C", "B", "A", "D"};
+  EXPECT_EQ(expected, output);
+}
+
+TEST(tf2_chainAsVector, LookupError_SourceFrameNotFound)
+{
+  tf2::BufferCore buffer;
+  std::vector<std::string> output;
+
+  // Attempt to get chain with non-existent source frame
+  EXPECT_THROW(
+      buffer._chainAsVector(
+          "target", tf2::timeFromSec(1.0),
+          "non_existent", tf2::timeFromSec(1.0),
+          "fixed",
+          output),
+      tf2::LookupException);
+}
+
+TEST(tf2_chainAsVector, ConnectivityError)
+{
+  tf2::BufferCore buffer;
+
+  // Add isolated transform C -> D (no connection to fixed frame A)
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.stamp.sec = 1;
+  transform.header.frame_id = "C";
+  transform.child_frame_id = "D";
+  transform.transform.rotation.w = 1.0;
+  ASSERT_TRUE(buffer.setTransform(transform, "test"));
+
+  std::vector<std::string> output;
+  // Attempt to get chain from D to A (fixed) which is disconnected
+  EXPECT_THROW(
+      buffer._chainAsVector(
+          "A", tf2::timeFromSec(1.0),
+          "D", tf2::timeFromSec(1.0),
+          "A",
+          output),
+      tf2::ConnectivityException);
+}
+
+TEST(tf2_chainAsVector, ExtrapolationError)
+{
+  tf2::BufferCore buffer;
+
+  // Add transform with timestamp at 2 seconds
+  geometry_msgs::msg::TransformStamped transform;
+  transform.header.stamp.sec = 2;
+  transform.header.frame_id = "A";
+  transform.child_frame_id = "B";
+  transform.transform.rotation.w = 1.0;
+  ASSERT_TRUE(buffer.setTransform(transform, "test"));
+
+  std::vector<std::string> output;
+  // Request transform at 1 second (before available time)
+  EXPECT_THROW(
+      buffer._chainAsVector(
+          "B", tf2::timeFromSec(1.0),
+          "A", tf2::timeFromSec(1.0),
+          "A",
+          output),
+      tf2::ExtrapolationException);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}