Fix for #589 - Should be able to transform with default timeout (#593)

When the timeout is default (0.0), it should NOT error. When the timeout is NOT default, it should raise an error.
ros2 · Jul 17, 2023 · b98963d · b98963d
1 parent 3229880
commit b98963d
Show file tree

Hide file tree

Showing 2 changed files with 45 additions and 2 deletions.
diff --git 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;
   }
 

diff --git 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<rclcpp::Clock>(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<rclcpp::Clock>(RCL_SYSTEM_TIME);