Implement waitForTransform timeout

Introduce CreateTimerInterface to abstract away the creation and management of timers.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

tf2_ros/CMakeLists.txt

@@ -34,6 +34,7 @@ include_directories(include
 # tf2_ros library
 add_library(${PROJECT_NAME} SHARED
   src/buffer.cpp
+  src/create_timer_ros.cpp
   src/transform_listener.cpp
   # src/buffer_client.cpp
   # src/buffer_server.cpp

tf2_ros/include/tf2_ros/buffer.h

@@ -32,7 +32,12 @@
 #ifndef TF2_ROS_BUFFER_H
 #define TF2_ROS_BUFFER_H
 
+#include <memory>
+#include <mutex>
+#include <unordered_map>
+
 #include <tf2_ros/buffer_interface.h>
+#include <tf2_ros/create_timer_interface.h>
 #include <tf2_ros/visibility_control.h>
 #include <tf2/buffer_core.h>
 #include <tf2_msgs/srv/frame_graph.hpp>
@@ -142,8 +147,19 @@ namespace tf2_ros
     waitForTransform(const std::string& target_frame, const std::string& source_frame, const tf2::TimePoint& time,
                      const tf2::Duration& timeout, TransformReadyCallback callback) override;
 
+    TF2_ROS_PUBLIC
+    inline void
+    setCreateTimerInterface(CreateTimerInterface::SharedPtr create_timer_interface)
+    {
+      timer_interface_ = create_timer_interface;
+    }
 
   private:
+    void timerCallback(const TimerHandle & timer_handle,
+                       std::shared_ptr<std::promise<geometry_msgs::msg::TransformStamped>> promise,
+                       TransformStampedFuture future,
+                       TransformReadyCallback callback);
+
     bool getFrames(tf2_msgs::srv::FrameGraph::Request& req, tf2_msgs::srv::FrameGraph::Response& res) ;
 
     void onTimeJump(const rcl_time_jump_t & jump);
@@ -157,6 +173,15 @@ namespace tf2_ros
     /// \brief A clock to use for time and sleeping
     rclcpp::Clock::SharedPtr clock_;
 
+    /// \brief Interface for creating timers
+    CreateTimerInterface::SharedPtr timer_interface_;
+
+    /// \brief A map from active timers to BufferCore request handles
+    std::unordered_map<TimerHandle, tf2::TransformableRequestHandle> timer_to_request_map_;
+
+    /// \brief A mutex on the timer_to_request_map_ data
+    std::mutex timer_to_request_map_mutex_;
+
     /// \brief Reference to a jump handler registered to the clock
     rclcpp::JumpHandler::SharedPtr jump_handler_;
   }; // class 

tf2_ros/include/tf2_ros/create_timer_interface.h

@@ -0,0 +1,122 @@
+/*
+ * Copyright (c) 2019, Open Source Robotics Foundation, Inc.
+ * 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, Inc. 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 OWNER 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.
+ */
+
+#ifndef TF2_ROS__CREATE_TIMER_INTERFACE_H
+#define TF2_ROS__CREATE_TIMER_INTERFACE_H
+
+#include <functional>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <tf2/time.h>
+
+#include <tf2_ros/visibility_control.h>
+
+namespace tf2_ros
+{
+
+using TimerHandle = uint64_t;
+using TimerCallbackType = std::function<void(const TimerHandle &)>;
+
+class InvalidTimerHandleException: public std::runtime_error
+{ 
+public:
+  TF2_ROS_PUBLIC
+  InvalidTimerHandleException(const std::string description)\
+  : std::runtime_error(description)
+  {
+  }
+};
+
+/**
+ * \brief Abstract interface for creating timers.
+ */
+class CreateTimerInterface
+{
+public:
+  using SharedPtr = std::shared_ptr<CreateTimerInterface>;
+  using ConstSharedPtr = std::shared_ptr<const CreateTimerInterface>;
+  using UniquePtr = std::unique_ptr<CreateTimerInterface>;
+
+  /**
+   * \brief Create a new timer.
+   *
+   * After creation, the timer will periodically execute the user-provided callback.
+   *
+   * \param clock The clock providing the current time
+   * \param period The interval at which the timer fires
+   * \param callback The callback function to execute every interval
+   */
+  TF2_ROS_PUBLIC
+  virtual TimerHandle
+  createTimer(
+    rclcpp::Clock::SharedPtr clock,
+    const tf2::Duration & period,
+    TimerCallbackType callback) = 0;
+
+  /**
+   * \brief Cancel a timer.
+   *
+   * The timer will stop executing user callbacks.
+   *
+   * \param timer_handle Handle to the timer to cancel
+   * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist
+   */
+  TF2_ROS_PUBLIC
+  virtual void
+  cancel(const TimerHandle & timer_handle) = 0;
+
+  /**
+   * \brief Reset the timer.
+   *
+   * The timer will reset and continue to execute user callbacks periodically.
+   *
+   * \param timer_handle Handle to the timer to reset
+   * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist
+   */
+  TF2_ROS_PUBLIC
+  virtual void
+  reset(const TimerHandle & timer_handle) = 0;
+
+  /**
+   * \brief Remove a timer.
+   *
+   * The timer will be canceled and removed from internal storage.
+   *
+   * \param timer_handle Handle to the timer to reset
+   * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist
+   */
+  TF2_ROS_PUBLIC
+  virtual void
+  remove(const TimerHandle & timer_handle) = 0;
+};  // class CreateTimerInterface
+
+}  // namespace tf2_ros
+
+#endif // TF2_ROS__CREATE_TIMER_INTERFACE_H

tf2_ros/include/tf2_ros/create_timer_ros.h

@@ -0,0 +1,124 @@
+/*
+ * Copyright (c) 2019, Open Source Robotics Foundation, Inc.
+ * 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, Inc. 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 OWNER 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.
+ */
+
+#ifndef TF2_ROS__CREATE_TIMER_ROS_H
+#define TF2_ROS__CREATE_TIMER_ROS_H
+
+#include <mutex>
+#include <unordered_map>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <tf2_ros/create_timer_interface.h>
+#include <tf2_ros/visibility_control.h>
+
+namespace tf2_ros
+{
+
+/**
+ * \brief Create and manage ROS timers.
+ *
+ * This class is thread safe.
+ */
+class CreateTimerROS : public virtual CreateTimerInterface
+{
+public:
+  CreateTimerROS(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers);
+
+  /**
+   * \brief Create a new timer.
+   *
+   * After creation, the timer will periodically execute the user-provided callback.
+   *
+   * \param clock The clock providing the current time
+   * \param period The interval at which the timer fires
+   * \param callback The callback function to execute every interval
+   */
+  TF2_ROS_PUBLIC
+  virtual TimerHandle
+  createTimer(
+    rclcpp::Clock::SharedPtr clock,
+    const tf2::Duration & period,
+    TimerCallbackType callback) override;
+
+  /**
+   * \brief Cancel a timer.
+   *
+   * The timer will stop executing user callbacks.
+   *
+   * \param timer_handle Handle to the timer to cancel
+   * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist
+   */
+  TF2_ROS_PUBLIC
+  virtual void
+  cancel(const TimerHandle & timer_handle) override;
+
+  /**
+   * \brief Reset the timer.
+   *
+   * The timer will reset and continue to execute user callbacks periodically.
+   *
+   * \param timer_handle Handle to the timer to reset
+   * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist
+   */
+  TF2_ROS_PUBLIC
+  virtual void
+  reset(const TimerHandle & timer_handle) override;
+
+  /**
+   * \brief Remove a timer.
+   *
+   * The timer will be canceled and removed from internal storage.
+   *
+   * \param timer_handle Handle to the timer to reset.
+   * \raises tf2_ros::InvalidTimerHandleException if the timer does not exist
+   */
+  TF2_ROS_PUBLIC
+  virtual void
+  remove(const TimerHandle & timer_handle) override;
+
+private:
+
+  void
+  timerCallback(
+    const TimerHandle & timer_handle,
+    TimerCallbackType callback);
+
+  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
+  rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_;
+  TimerHandle next_timer_handle_index_;
+  std::unordered_map<TimerHandle, rclcpp::TimerBase::SharedPtr> timers_map_;
+  std::mutex timers_map_mutex_;
+};  // class CreateTimerROS
+
+}  // namespace tf2_ros
+
+#endif // TF2_ROS__CREATE_TIMER_INTERFACE_H

tf2_ros/src/buffer.cpp

@@ -34,6 +34,7 @@
 
 #include <exception>
 #include <limits>
+#include <mutex>
 #include <sstream>
 #include <thread>
 
@@ -48,7 +49,7 @@ namespace tf2_ros
 {
 
 Buffer::Buffer(rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time) :
-  BufferCore(cache_time), clock_(clock)
+  BufferCore(cache_time), clock_(clock), timer_interface_(nullptr)
 {
   if (nullptr == clock_)
   {
@@ -192,11 +193,15 @@ Buffer::canTransform(const std::string& target_frame, const tf2::TimePoint& targ
 
 TransformStampedFuture
 Buffer::waitForTransform(const std::string& target_frame, const std::string& source_frame, const tf2::TimePoint& time,
-                 const tf2::Duration& timeout, TransformReadyCallback callback)
+                         const tf2::Duration& timeout, TransformReadyCallback callback)
 {
-  // TODO(jacobperron): Implement timeout
+  if (nullptr == timer_interface_) {
+    throw std::runtime_error("timer interface not set before call to waitForTransform");
+  }
+
   auto promise = std::make_shared<std::promise<geometry_msgs::msg::TransformStamped>>();
   TransformStampedFuture future(promise->get_future());
+
   auto cb_handle = addTransformableCallback([&, promise, callback, future](
     tf2::TransformableRequestHandle request_handle, const std::string& target_frame,
     const std::string& source_frame, tf2::TimePoint time, tf2::TransformableResult result)
@@ -222,10 +227,37 @@ Buffer::waitForTransform(const std::string& target_frame, const std::string& sou
     // Never transformable
     promise->set_exception(std::make_exception_ptr<tf2::LookupException>(
           "Failed to transform from " + source_frame + " to " + target_frame));
+  } else {
+    std::lock_guard<std::mutex> lock(timer_to_request_map_mutex_);
+    auto timer_handle = timer_interface_->createTimer(
+      clock_,
+      timeout,
+      std::bind(&Buffer::timerCallback, this, std::placeholders::_1, promise, future, callback));
+
+    // Save association between timer and request handle
+    timer_to_request_map_[timer_handle] = handle;
   }
   return future;
 }
 
+void
+Buffer::timerCallback(const TimerHandle & timer_handle,
+                      std::shared_ptr<std::promise<geometry_msgs::msg::TransformStamped>> promise,
+                      TransformStampedFuture future,
+                      TransformReadyCallback callback)
+{
+  tf2::TransformableRequestHandle request_handle;
+  {
+    std::lock_guard<std::mutex> lock(timer_to_request_map_mutex_);
+    request_handle = timer_to_request_map_.at(timer_handle);
+    timer_to_request_map_.erase(timer_handle);
+  }
+  cancelTransformableRequest(request_handle);
+  promise->set_exception(
+    std::make_exception_ptr<tf2::TimeoutException>(std::string("Timed out waiting for transform")));
+  callback(future);
+  timer_interface_->remove(timer_handle);
+}
 
 bool Buffer::getFrames(tf2_msgs::srv::FrameGraph::Request& req, tf2_msgs::srv::FrameGraph::Response& res) 
 {