Introduce and use is_valid and wait_for_valid methods

Signed-off-by: methylDragon <methylDragon@gmail.com>

fuse_core/include/fuse_core/time.h

@@ -56,6 +56,7 @@
 #include <iostream>
 #include <stdexcept>
 
+#include "rclcpp/contexts/default_context.hpp"
 #include <rclcpp/clock.hpp>
 #include <rclcpp/duration.hpp>
 #include <rclcpp/time.hpp>
@@ -136,17 +137,49 @@ std::ostream& operator<<(std::ostream& os, const fuse_core::TimeStamp& timestamp
 
 
 /**
- * @brief Wait for time to be valid (non-zero)
- *
- * Logic adapted from: http://docs.ros.org/en/noetic/api/rostime/html/src_2time_8cpp_source.html
+ * @brief Check if clock's time is valid (non-zero)
  *
  * @param[in] clock    An rclcpp::Clock to measure time against
- * @param[in] timeout  An optional timeout
+ * @param[in] context  The context to wait in
+ *
+ * @return valid  true if clock was or became valid
+ */
+// TOOD(CH3): Replace with rclcpp's implementation when https://github.com/ros2/rclcpp/pull/2040 is in
+bool is_valid(
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Context::SharedPtr context = rclcpp::contexts::get_global_default_context());
+
+
+/**
+ * @brief Wait for clock's time to be valid (non-zero)
  *
- * @param[out] valid   Whether the clock's time was valid
+ * @param[in] clock         An rclcpp::Clock to measure time against
+ * @param[in] context       The context to wait in
+ *
+ * @return valid  true if clock was or became valid
  */
-bool waitForValid(rclcpp::Clock clock, rclcpp::Duration timeout=rclcpp::Duration(0, 0));
+// TOOD(CH3): Replace with rclcpp's implementation when https://github.com/ros2/rclcpp/pull/2040 is in
+bool wait_for_valid(
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Context::SharedPtr context = rclcpp::contexts::get_global_default_context());
+
 
+/**
+ * @brief Wait for clock's time to be valid (non-zero), with timeout
+ *
+ * @param[in] clock         An rclcpp::Clock to measure time against
+ * @param[in] timeout       The maximum time to wait for
+ * @param[in] context       The context to wait in
+ * @param[in] wait_tick_ns  The time to wait between each iteration of the wait loop (in ns)
+ *
+ * @return valid  true if clock was or became valid
+ */
+// TOOD(CH3): Replace with rclcpp's implementation when https://github.com/ros2/rclcpp/pull/2040 is in
+bool wait_for_valid(
+  rclcpp::Clock::SharedPtr clock,
+  const rclcpp::Duration & timeout,
+  rclcpp::Context::SharedPtr context = rclcpp::contexts::get_global_default_context(),
+  const rclcpp::Duration & wait_tick_ns = rclcpp::Duration(0, static_cast<uint32_t>(1e7)));
 }
 
 #endif  // FUSE_CORE_TIME_H

fuse_core/src/time.cpp

@@ -47,6 +47,8 @@
  */
 
 #include <fuse_core/time.h>
+
+#include "rcl/time.h"
 #include <rclcpp/utilities.hpp>
 
 using chrono_ns_time_point_t =
@@ -100,37 +102,78 @@ std::ostream& operator<<(std::ostream& os, const fuse_core::TimeStamp& timestamp
 }
 
 
-bool isValid(rclcpp::Clock::SharedPtr clock)
+bool is_valid(rclcpp::Clock::SharedPtr clock, rclcpp::Context::SharedPtr context)
 {
+  // Checks for null pointer, missing get_now() implementation, and RCL_CLOCK_UNINITIALIZED
+  if (!rcl_clock_valid(clock->get_clock_handle())) {
+    return false;
+  }
+
   switch (clock->get_clock_type()) {
-    case RCL_CLOCK_UNINITIALIZED:
-      return false;
-    case RCL_SYSTEM_TIME:
-      return true;
     case RCL_ROS_TIME:
-      return clock->now().nanoseconds() == 0;
+    case RCL_STEADY_TIME:
+    case RCL_SYSTEM_TIME:
+      return clock->now().nanoseconds() > 0;
+
+    // By right we shouldn't even get to this block, but these cases are included for completeness
+    case RCL_CLOCK_UNINITIALIZED:
     default:
       return false;
   }
 }
 
 
-// Logic adapted from: http://docs.ros.org/en/noetic/api/rostime/html/src_2time_8cpp_source.html
-bool waitForValid(rclcpp::Clock::SharedPtr clock, rclcpp::Duration timeout)
+bool wait_for_valid(
+  rclcpp::Clock::SharedPtr clock,
+  rclcpp::Context::SharedPtr context)
+{
+  if (!context || !context->is_valid()) {
+    throw std::runtime_error("context cannot be slept with because it's invalid");
+  }
+
+  if (!rcl_clock_valid(clock->get_clock_handle())) {
+    throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
+  }
+
+  if (is_valid(clock, context)) {
+    return true;
+  } else {
+    // Wait until the first valid time
+    return clock->sleep_until(rclcpp::Time(0, 1, clock->get_clock_type()), context);
+  }
+}
+
+
+bool wait_for_valid(
+  rclcpp::Clock::SharedPtr clock,
+  const rclcpp::Duration & timeout,
+  rclcpp::Context::SharedPtr context,
+  const rclcpp::Duration & wait_tick_ns)
 {
-   rclcpp::Time start = clock->now();
-   while (clock->now().nanoseconds() == 0 && rclcpp::ok()) {
-     rclcpp::sleep_for(std::chrono::nanoseconds(timeout.nanoseconds()));
-
-     if (timeout > rclcpp::Duration(0, 0) && (clock->now() - start > timeout)) {
-       return false;
-     }
-   }
-
-   if (!rclcpp::ok()) {
-     return false;
-   }
-   return true;
+  if (!context || !context->is_valid()) {
+    throw std::runtime_error("context cannot be slept with because it's invalid");
+  }
+
+  if (!rcl_clock_valid(clock->get_clock_handle())) {
+    throw std::runtime_error("clock cannot be waited on as it is not rcl_clock_valid");
+  }
+
+  rclcpp::Clock timeout_clock = rclcpp::Clock(RCL_STEADY_TIME);
+  rclcpp::Time start = timeout_clock.now();
+
+  while (!is_valid(clock, context) && context->is_valid()) {  // Context check checks for rclcpp::shutdown()
+    if (timeout < wait_tick_ns) {
+      timeout_clock.sleep_for(timeout);
+    } else {
+      timeout_clock.sleep_for(rclcpp::Duration(wait_tick_ns));
+    }
+
+    if (timeout_clock.now() - start > timeout) {
+      return is_valid(clock, context);
+    }
+  }
+
+  return is_valid(clock, context);
 }
 
 }

fuse_core/test/test_throttled_callback.cpp

@@ -185,7 +185,7 @@ TEST(ThrottledCallback, NoDroppedMessagesIfThrottlePeriodIsZero)
 {
   // Time should be valid after ros::init() returns in main(). But it doesn't hurt to verify.
   // TODO(CH3): Refactor this with a passed in node
-  // ASSERT_TRUE(fuse_core::waitForValid(<NODE_HERE>->get_clock(), rclcpp::Duration::from_seconds(1.0)));
+  // ASSERT_TRUE(fuse_core::wait_for_valid(<NODE_HERE>->get_clock(), rclcpp::Duration::from_seconds(1.0)));
 
   // Start sensor model to listen to messages:
   const rclcpp::Duration throttled_period(0.0);
@@ -207,7 +207,7 @@ TEST(ThrottledCallback, DropMessagesIfThrottlePeriodIsGreaterThanPublishPeriod)
 {
   // Time should be valid after ros::init() returns in main(). But it doesn't hurt to verify.
   // TODO(CH3): Refactor this with a passed in node
-  // ASSERT_TRUE(fuse_core::waitForValid(<NODE_HERE>->get_clock(), rclcpp::Duration::from_seconds(1.0)));
+  // ASSERT_TRUE(fuse_core::wait_for_valid(<NODE_HERE>->get_clock(), rclcpp::Duration::from_seconds(1.0)));
 
   // Start sensor model to listen to messages:
   const rclcpp::Duration throttled_period(0.2);
@@ -234,7 +234,7 @@ TEST(ThrottledCallback, AlwaysKeepFirstMessageEvenIfThrottlePeriodIsTooLarge)
 {
   // Time should be valid after ros::init() returns in main(). But it doesn't hurt to verify.
   // TODO(CH3): Refactor this with a passed in node
-  // ASSERT_TRUE(fuse_core::waitForValid(<NODE_HERE>->get_clock(), rclcpp::Duration::from_seconds(1.0)));
+  // ASSERT_TRUE(fuse_core::wait_for_valid(<NODE_HERE>->get_clock(), rclcpp::Duration::from_seconds(1.0)));
 
   // Start sensor model to listen to messages:
   const rclcpp::Duration throttled_period(10.0);

fuse_optimizers/src/optimizer.cpp

@@ -105,7 +105,7 @@ Optimizer::Optimizer(
   diagnostic_updater_.setHardwareID("fuse");
 
   // Wait for a valid time before loading any of the plugins
-  // fuse_core::waitForValid(this->get_node_clock_interface()->get_clock());  // TODO(CH3): Uncomment this when ready
+  // fuse_core::wait_for_valid(this->get_node_clock_interface()->get_clock());  // TODO(CH3): Uncomment this when ready
 
   // Load all configured plugins
   loadMotionModels();
@@ -478,7 +478,7 @@ void Optimizer::stopPlugins()
 
 void Optimizer::setDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& status)
 {
-  if (!fuse_core::isValid(this->get_node_clock_interface()->get_clock()))
+  if (!fuse_core::is_valid(this->get_node_clock_interface()->get_clock()))
   {
     status.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Waiting for valid ROS time");
     return;

fuse_optimizers/test/test_fixed_lag_ignition.cpp

@@ -46,7 +46,7 @@ TEST(FixedLagIgnition, SetInitialState)
   auto relative_pose_publisher = node_handle.advertise<geometry_msgs::PoseWithCovarianceStamped>("/relative_pose", 1);
 
   // Time should be valid after rclcpp::init() returns in main(). But it doesn't hurt to verify.
-  ASSERT_TRUE(fuse_core::waitForValid(node->get_clock(), rclcpp::Duration::from_seconds(1.0)));
+  ASSERT_TRUE(fuse_core::wait_for_valid(node->get_clock(), rclcpp::Duration::from_seconds(1.0)));
 
   // Wait for the optimizer to be ready
   ASSERT_TRUE(ros::service::waitForService("/fixed_lag/set_pose", rclcpp::Duration::from_seconds(1.0)));