From c24de7f68bde0f526cdffe1854401910c616e538 Mon Sep 17 00:00:00 2001 From: cwecht Date: Wed, 21 Feb 2018 22:33:22 +0100 Subject: [PATCH] =?UTF-8?q?tf2=5Fros::Buffer:=20canTransform=20can=20now?= =?UTF-8?q?=20deal=20with=20timeouts=20smaller=20than=E2=80=A6=20(#286)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * tf2_ros::Buffer: canTransform can now deal with timeouts smaller than 10ms by using the hunderdth of the timeout for sleeping --- tf2_ros/src/buffer.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/tf2_ros/src/buffer.cpp b/tf2_ros/src/buffer.cpp index b78823a4e..803030165 100644 --- a/tf2_ros/src/buffer.cpp +++ b/tf2_ros/src/buffer.cpp @@ -38,6 +38,8 @@ namespace tf2_ros { +static const double CAN_TRANSFORM_POLLING_SCALE = 0.01; + Buffer::Buffer(ros::Duration cache_time, bool debug) : BufferCore(cache_time) { @@ -128,12 +130,13 @@ Buffer::canTransform(const std::string& target_frame, const std::string& source_ // poll for transform if timeout is set ros::Time start_time = now_fallback_to_wall(); + const ros::Duration sleep_duration = timeout * CAN_TRANSFORM_POLLING_SCALE; while (now_fallback_to_wall() < start_time + timeout && !canTransform(target_frame, source_frame, time) && (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) && //don't wait when we detect a bag loop (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf) { - sleep_fallback_to_wall(ros::Duration(0.01)); + sleep_fallback_to_wall(sleep_duration); } bool retval = canTransform(target_frame, source_frame, time, errstr); conditionally_append_timeout_info(errstr, start_time, timeout); @@ -157,12 +160,13 @@ Buffer::canTransform(const std::string& target_frame, const ros::Time& target_ti // poll for transform if timeout is set ros::Time start_time = now_fallback_to_wall(); + const ros::Duration sleep_duration = timeout * CAN_TRANSFORM_POLLING_SCALE; while (now_fallback_to_wall() < start_time + timeout && !canTransform(target_frame, target_time, source_frame, source_time, fixed_frame) && (now_fallback_to_wall()+ros::Duration(3.0) >= start_time) && //don't wait when we detect a bag loop (ros::ok() || !ros::isInitialized())) // Make sure we haven't been stopped (won't work for pytf) { - sleep_fallback_to_wall(ros::Duration(0.01)); + sleep_fallback_to_wall(sleep_duration); } bool retval = canTransform(target_frame, target_time, source_frame, source_time, fixed_frame, errstr); conditionally_append_timeout_info(errstr, start_time, timeout);