From f5db32ee4607fa7414b4e41f6823219d70d48f0a Mon Sep 17 00:00:00 2001 From: vineet131 Date: Wed, 29 Mar 2023 22:42:48 +0900 Subject: [PATCH 1/2] Fix for upstream geometry2#589 --- tf2_ros/src/buffer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2_ros/src/buffer.cpp b/tf2_ros/src/buffer.cpp index 2131de689..5a7fe7810 100644 --- a/tf2_ros/src/buffer.cpp +++ b/tf2_ros/src/buffer.cpp @@ -141,7 +141,7 @@ Buffer::canTransform( const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, const tf2::Duration timeout, std::string * errstr) const { - if (!checkAndErrorDedicatedThreadPresent(errstr)) { + if (timeout != tf2::durationFromSec(0.0) && !checkAndErrorDedicatedThreadPresent(errstr)) { return false; } @@ -171,7 +171,7 @@ Buffer::canTransform( const std::string & source_frame, const tf2::TimePoint & source_time, const std::string & fixed_frame, const tf2::Duration timeout, std::string * errstr) const { - if (!checkAndErrorDedicatedThreadPresent(errstr)) { + if (timeout != tf2::durationFromSec(0.0) && !checkAndErrorDedicatedThreadPresent(errstr)) { return false; } From c19513514bb470116de8712a594a4d53c505dd41 Mon Sep 17 00:00:00 2001 From: vineet Date: Sun, 16 Jul 2023 01:31:51 +0900 Subject: [PATCH 2/2] Add test for timeouts on non-dedicated thread When the timeout is default (0.0), it should NOT error. When the timeout is NOT default, it should raise an error. --- tf2_ros/test/test_buffer.cpp | 43 ++++++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/tf2_ros/test/test_buffer.cpp b/tf2_ros/test/test_buffer.cpp index 6b5588485..46e998787 100644 --- a/tf2_ros/test/test_buffer.cpp +++ b/tf2_ros/test/test_buffer.cpp @@ -138,6 +138,49 @@ TEST(test_buffer, can_transform_valid_transform) EXPECT_DOUBLE_EQ(transform.transform.translation.z, output_rclcpp.transform.translation.z); } +TEST(test_buffer, can_transform_without_dedicated_thread) +{ + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + buffer.setUsingDedicatedThread(false); + + rclcpp::Time rclcpp_time = clock->now(); + tf2::TimePoint tf2_time(std::chrono::nanoseconds(rclcpp_time.nanoseconds())); + + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = "foo"; + transform.header.stamp = builtin_interfaces::msg::Time(rclcpp_time); + transform.child_frame_id = "bar"; + transform.transform.translation.x = 42.0; + transform.transform.translation.y = -3.14; + transform.transform.translation.z = 0.0; + transform.transform.rotation.w = 1.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + + EXPECT_TRUE(buffer.setTransform(transform, "unittest")); + + // Should NOT error with default timeout + EXPECT_TRUE(buffer.canTransform("bar", "foo", tf2_time)); + EXPECT_TRUE(buffer.canTransform("bar", "foo", rclcpp_time)); + // Should error when timeout is not default + EXPECT_FALSE(buffer.canTransform("bar", "foo", tf2_time, std::chrono::seconds(2))); + EXPECT_FALSE(buffer.canTransform("bar", "foo", rclcpp_time, rclcpp::Duration::from_seconds(1.0))); + + auto output = buffer.lookupTransform("foo", "bar", tf2_time); + EXPECT_STREQ(transform.child_frame_id.c_str(), output.child_frame_id.c_str()); + EXPECT_DOUBLE_EQ(transform.transform.translation.x, output.transform.translation.x); + EXPECT_DOUBLE_EQ(transform.transform.translation.y, output.transform.translation.y); + EXPECT_DOUBLE_EQ(transform.transform.translation.z, output.transform.translation.z); + + auto output_rclcpp = buffer.lookupTransform("foo", "bar", rclcpp_time); + EXPECT_STREQ(transform.child_frame_id.c_str(), output_rclcpp.child_frame_id.c_str()); + EXPECT_DOUBLE_EQ(transform.transform.translation.x, output_rclcpp.transform.translation.x); + EXPECT_DOUBLE_EQ(transform.transform.translation.y, output_rclcpp.transform.translation.y); + EXPECT_DOUBLE_EQ(transform.transform.translation.z, output_rclcpp.transform.translation.z); +} + TEST(test_buffer, wait_for_transform_valid) { rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME);