Synchronize rcl and std::chrono steady clocks in Clock::sleep_until (#…

…1830)

* Synchronize RCL and std::chrono steady clocks

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Warn about gcc steady clock bugs

Signed-off-by: Shane Loretz <sloretz@openrobotics.org>

* Try to make warning message clearer

Signed-off-by: Shane Loretz <sloretz@osrfoundation.org>

rclcpp/include/rclcpp/clock.hpp

@@ -88,6 +88,16 @@ class Clock
    *     false. There is not a consistent choice of sleeping time when the time source changes,
    *     so this is up to the caller to call again if needed.
    *
+   * \warning When using gcc < 10 or when using gcc >= 10 and pthreads lacks the function
+   *    `pthread_cond_clockwait`, steady clocks may sleep using the system clock.
+   *    If so, steady clock sleep times can be affected by system clock time jumps.
+   *    Depending on the steady clock's epoch and resolution in comparison to the system clock's,
+   *    an overflow when converting steady clock durations to system clock times may cause
+   *    undefined behavior.
+   *    For more info see these issues:
+   *    https://gcc.gnu.org/bugzilla/show_bug.cgi?id=41861
+   *    https://gcc.gnu.org/bugzilla/show_bug.cgi?id=58931
+   *
    * \param until absolute time according to current clock type to sleep until.
    * \param context the rclcpp context the clock should use to check that ROS is still initialized.
    * \return true immediately if `until` is in the past

rclcpp/src/rclcpp/clock.cpp

@@ -105,13 +105,17 @@ Clock::sleep_until(Time until, Context::SharedPtr context)
     });
 
   if (this_clock_type == RCL_STEADY_TIME) {
-    auto steady_time = std::chrono::steady_clock::time_point(
-      std::chrono::nanoseconds(until.nanoseconds()));
+    // Synchronize because RCL steady clock epoch might differ from chrono::steady_clock epoch
+    const Time rcl_entry = now();
+    const std::chrono::steady_clock::time_point chrono_entry = std::chrono::steady_clock::now();
+    const Duration delta_t = until - rcl_entry;
+    const std::chrono::steady_clock::time_point chrono_until =
+      chrono_entry + std::chrono::nanoseconds(delta_t.nanoseconds());
 
     // loop over spurious wakeups but notice shutdown
     std::unique_lock lock(impl_->clock_mutex_);
     while (now() < until && context->is_valid()) {
-      cv.wait_until(lock, steady_time);
+      cv.wait_until(lock, chrono_until);
     }
   } else if (this_clock_type == RCL_SYSTEM_TIME) {
     auto system_time = std::chrono::system_clock::time_point(