Add Clock.sleep_until

rclpy/CMakeLists.txt

@@ -109,6 +109,7 @@ pybind11_add_module(_rclpy_pybind11 SHARED
   src/rclpy/context.cpp
   src/rclpy/destroyable.cpp
   src/rclpy/duration.cpp
+  src/rclpy/event.cpp
   src/rclpy/exceptions.cpp
   src/rclpy/graph.cpp
   src/rclpy/guard_condition.cpp

rclpy/rclpy/clock.py

@@ -15,7 +15,9 @@
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 
 from .duration import Duration
+from .exceptions import NotInitializedException
 from .time import Time
+from .utilities import get_default_context
 
 
 ClockType = _rclpy.ClockType
@@ -172,6 +174,71 @@ def callback_shim(jump_dict):
             clock=self, threshold=threshold, pre_callback=pre_callback,
             post_callback=post_callback)
 
+    def sleep_until(self, until: Time, context=None) -> bool:
+        """
+        Sleep until a Time on this Clock is reached.
+
+        When using a ROSClock, this may sleep forever if the TimeSource is misconfigured and the
+        context is never shut down.
+        ROS time being activated or deactivated causes this function to cease sleeping and return
+        False.
+
+        :param until: Time at which this function should stop sleeping.
+        :param context: Context which when shut down will cause this sleep to wake early.
+            If context is None, then the default context is used.
+        :return: True if until was reached, or False if it woke for another reason.
+        :raises ValueError: until is specified for a different type of clock than this one.
+        :raises NotInitializedException: context has not been initialized or is shutdown.
+        """
+        if context is None:
+            context = get_default_context()
+
+        if not context.ok():
+            raise NotInitializedException()
+
+        if until.clock_type != self._clock_type:
+            raise ValueError("until's clock type does not match this clock's type")
+
+        event = _rclpy.Event()
+        time_source_changed = False
+
+        def on_time_jump(time_jump: TimeJump):
+            """Wake when time jumps and is past target time."""
+            nonlocal time_source_changed
+
+            # ROS time being activated or deactivated changes the epoch, so sleep
+            # time loses its meaning
+            time_source_changed = (
+                time_source_changed or
+                ClockChange.ROS_TIME_ACTIVATED == time_jump.clock_change or
+                ClockChange.ROS_TIME_DEACTIVATED == time_jump.clock_change)
+
+            if time_source_changed or self.now() >= until:
+                event.set()
+
+        def on_shutdown():
+            """Wake when context is shut down."""
+            event.set()
+
+        context.on_shutdown(on_shutdown)
+
+        threshold = JumpThreshold(
+            min_forward=Duration(nanoseconds=1),
+            min_backward=None,
+            on_clock_change=True)
+        with self.create_jump_callback(threshold, post_callback=on_time_jump):
+            if ClockType.SYSTEM_TIME == self._clock_type:
+                event.wait_until_system(self.__clock, until._time_handle)
+            elif ClockType.STEADY_TIME == self._clock_type:
+                event.wait_until_steady(self.__clock, until._time_handle)
+            elif ClockType.ROS_TIME == self._clock_type:
+                event.wait_until_ros(self.__clock, until._time_handle)
+
+        if not context.ok() or time_source_changed:
+            return False
+
+        return self.now() >= until
+
 
 class ROSClock(Clock):
 

rclpy/src/rclpy/_rclpy_pybind11.cpp

@@ -25,6 +25,7 @@
 #include "context.hpp"
 #include "destroyable.hpp"
 #include "duration.hpp"
+#include "event.hpp"
 #include "exceptions.hpp"
 #include "graph.hpp"
 #include "guard_condition.hpp"
@@ -228,4 +229,5 @@ PYBIND11_MODULE(_rclpy_pybind11, m) {
 
   rclpy::define_logging_api(m);
   rclpy::define_signal_handler_api(m);
+  rclpy::define_event(m);
 }

rclpy/src/rclpy/event.cpp

@@ -0,0 +1,120 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <pybind11/pybind11.h>
+
+#include <rcl/error_handling.h>
+#include <rcl/rcl.h>
+#include <rcl/time.h>
+#include <rcl/types.h>
+
+#include <condition_variable>
+#include <cstring>
+#include <memory>
+#include <stdexcept>
+
+#include "event.hpp"
+
+namespace py = pybind11;
+
+namespace rclpy
+{
+template<typename ClockType>
+void Event::wait_until(std::shared_ptr<Clock> clock, rcl_time_point_t until)
+{
+  // Synchronize because clock epochs might differ
+  const rcl_time_point_t rcl_entry = clock->get_now();
+  const typename ClockType::time_point chrono_entry = ClockType::now();
+
+  rcl_duration_t delta_t;
+  rcl_ret_t ret = rcl_difference_times(&rcl_entry, &until, &delta_t);
+
+  if (RCL_RET_OK != ret) {
+    throw RCLError("failed to subtract times");
+  }
+
+  // Cast because system clock resolution is be too big for nanoseconds on Windows & OSX
+  const typename ClockType::time_point chrono_until = chrono_entry +
+    std::chrono::duration_cast<typename ClockType::duration>(
+    std::chrono::nanoseconds(delta_t.nanoseconds));
+
+  // Could be a long wait, release the gil
+  py::gil_scoped_release release;
+  std::unique_lock<std::mutex> lock(mutex_);
+  cv_.wait_until(lock, chrono_until, [this]() {return state_;});
+}
+
+void Event::wait_until_ros(std::shared_ptr<Clock> clock, rcl_time_point_t until)
+{
+  // Check if ROS time is enabled in C++ to avoid TOCTTOU with TimeSource by holding GIL
+  if (clock->get_ros_time_override_is_enabled()) {
+    // Could be a long wait, release the gil
+    py::gil_scoped_release release;
+    std::unique_lock<std::mutex> lock(mutex_);
+    // Caller must have setup a time jump callback to wake this event
+    cv_.wait(lock, [this]() {return state_;});
+  } else {
+    // ROS time not enabled is system time
+    wait_until<std::chrono::system_clock>(clock, until);
+  }
+}
+
+bool Event::is_set()
+{
+  std::unique_lock<std::mutex> lock(mutex_);
+  return state_;
+}
+
+void Event::set()
+{
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    state_ = true;
+  }
+  cv_.notify_all();
+}
+
+void Event::clear()
+{
+  {
+    std::unique_lock<std::mutex> lock(mutex_);
+    state_ = false;
+  }
+  cv_.notify_all();
+}
+
+void define_event(py::object module)
+{
+  py::class_<Event>(module, "Event")
+  .def(py::init())
+  .def(
+    "wait_until_steady", &Event::wait_until<std::chrono::steady_clock>,
+    "Wait for the event to be set (monotonic wait)")
+  .def(
+    "wait_until_system", &Event::wait_until<std::chrono::system_clock>,
+    "Wait for the event to be set (system timed wait)")
+  .def(
+    "wait_until_ros", &Event::wait_until_ros,
+    "Wait for the event to be set (ROS timed wait)")
+  .def(
+    "is_set", &Event::is_set,
+    "Return True if the event is set, False otherwise.")
+  .def(
+    "set", &Event::set,
+    "Set the event, waking all those who wait on it.")
+  .def(
+    "clear", &Event::clear,
+    "Unset the event.");
+}
+}  // namespace rclpy

rclpy/src/rclpy/event.hpp

@@ -0,0 +1,71 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCLPY__EVENT_HPP_
+#define RCLPY__EVENT_HPP_
+
+#include <pybind11/pybind11.h>
+
+#include <rcl/error_handling.h>
+#include <rcl/rcl.h>
+#include <rcl/time.h>
+#include <rcl/types.h>
+
+#include <condition_variable>
+#include <cstring>
+#include <memory>
+#include <stdexcept>
+
+#include "clock.hpp"
+
+namespace py = pybind11;
+
+namespace rclpy
+{
+class Event
+{
+public:
+  /// Wait until a time specified by a system or steady clock.
+  /// \param clock the clock to use for time synchronization with until
+  template<typename ClockType>
+  void wait_until(std::shared_ptr<Clock> clock, rcl_time_point_t until);
+
+  /// Wait until a time specified by a ROS clock.
+  /// \warning the caller is responsible for creating a time jump callback to set this event when
+  /// the target ROS time is reached.
+  /// when a given ROS time is reached.
+  /// \param clock the clock to use for time synchronization.
+  void wait_until_ros(std::shared_ptr<Clock> clock, rcl_time_point_t until);
+
+  /// Indicate if the Event is set.
+  /// \return True if the Event is set.
+  bool is_set();
+
+  /// Set the event.
+  void set();
+
+  /// Clear the event.
+  void clear();
+
+private:
+  bool state_ = false;
+  std::mutex mutex_;
+  std::condition_variable cv_;
+};
+
+/// Define a pybind11 wrapper for an rclpy::Event
+void define_event(py::object module);
+}  // namespace rclpy
+
+#endif  // RCLPY__EVENT_HPP_