diff --git a/tf2/CMakeLists.txt b/tf2/CMakeLists.txt index 8a666920c..1cf94247d 100644 --- a/tf2/CMakeLists.txt +++ b/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) diff --git a/tf2/src/buffer_core.cpp b/tf2/src/buffer_core.cpp index 58d0e731e..fbf9b0554 100644 --- a/tf2/src/buffer_core.cpp +++ b/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 target_frame_chain; - retval = walkToTopParent( - accum, target_time, target_id, fixed_id, &error_string, - &target_frame_chain); + std::vector 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 diff --git a/tf2/test/chainAsVector.cpp b/tf2/test/chainAsVector.cpp new file mode 100644 index 000000000..d9640d826 --- /dev/null +++ b/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 + +#include +#include +#include + +#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 output; + buffer._chainAsVector( + "D", tf2::timeFromSec(1.0), // target + "C", tf2::timeFromSec(1.0), // source + "A", // fixed + output); + + // Verify the resulting chain + std::vector expected{"C", "B", "A", "D"}; + EXPECT_EQ(expected, output); +} + +TEST(tf2_chainAsVector, LookupError_SourceFrameNotFound) +{ + tf2::BufferCore buffer; + std::vector 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 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 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(); +} \ No newline at end of file