diff --git a/tf2_ros/src/tf2_ros/buffer.py b/tf2_ros/src/tf2_ros/buffer.py index 2f0e5660bc..0959329a23 100644 --- a/tf2_ros/src/tf2_ros/buffer.py +++ b/tf2_ros/src/tf2_ros/buffer.py @@ -218,6 +218,18 @@ def can_transform_full(self, target_frame, target_time, source_frame, source_tim return core_result return core_result[0] + def _wait_for_transform_async(self, target_Frame, source_frame, time, callback): + fut = rclpy.task.Future() + if self.can_transform_core(target_frame, source_frame, time)[0]: + # Short cut, the transform is available + fut.set_result(True) + return fut + + self._new_data_callbacks.append(callback) + fut.add_done_callback(lambda _: self._remove_callback(callback)) + + return fut + def wait_for_transform_async(self, target_frame, source_frame, time): """ Wait for a transform from the source frame to the target frame to become possible. @@ -228,12 +240,6 @@ def wait_for_transform_async(self, target_frame, source_frame, time): :return: A future that becomes true when the transform is available :rtype: rclpy.task.Future """ - fut = rclpy.task.Future() - if self.can_transform_core(target_frame, source_frame, time)[0]: - # Short cut, the transform is available - fut.set_result(True) - return fut - def _on_new_data(): try: if self.can_transform_core(target_frame, source_frame, time)[0]: @@ -241,10 +247,8 @@ def _on_new_data(): except BaseException as e: fut.set_exception(e) - self._new_data_callbacks.append(_on_new_data) - fut.add_done_callback(lambda _: self._remove_callback(_on_new_data)) + return _wait_for_transform_async(self, target_frame, source_frame, time, _on_new_data) - return fut def wait_for_transform_full_async(self, target_frame, target_time, source_frame, source_time, fixed_frame): """ @@ -275,3 +279,6 @@ def _on_new_data(): fut.add_done_callback(lambda _: self._remove_callback(_on_new_data)) return fut + + def wait_for_transform(target_frame, source_frame, time, callback): + return _wait_for_transform_async(target_frame, source_frame, time, callback)