Add new RCL_RAW_STEADY_TIME changes

Signed-off-by: Sai Kishor Kothakota <[email protected]>

Author: saikishor

rclcpp/include/rclcpp/timer.hpp

@@ -338,7 +338,8 @@ class GenericTimer : public TimerBase
   bool
   is_steady() override
   {
-    return clock_->get_clock_type() == RCL_STEADY_TIME;
+    return clock_->get_clock_type() == RCL_STEADY_TIME ||
+           clock_->get_clock_type() == RCL_RAW_STEADY_TIME;
   }
 
 protected:

rclcpp/src/rclcpp/clock.cpp

@@ -120,6 +120,20 @@ Clock::sleep_until(
     const std::chrono::steady_clock::time_point chrono_until =
       chrono_entry + std::chrono::nanoseconds(delta_t.nanoseconds());
 
+    // loop over spurious wakeups but notice shutdown or stop of sleep
+    std::unique_lock lock(impl_->wait_mutex_);
+    while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid()) {
+      impl_->cv_.wait_until(lock, chrono_until);
+    }
+    impl_->stop_sleeping_ = false;
+  } else if (this_clock_type == RCL_RAW_STEADY_TIME) {
+    // 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 or stop of sleep
     std::unique_lock lock(impl_->wait_mutex_);
     while (now() < until && !impl_->stop_sleeping_ && !impl_->shutdown_ && context->is_valid()) {
@@ -469,6 +483,7 @@ class ClockWaiter::ClockWaiterImpl
         return wait_until_ros_time(lock, abs_time, pred);
         break;
       case RCL_STEADY_TIME:
+      case RCL_RAW_STEADY_TIME:
         return wait_until_steady_time(lock, abs_time, pred);
         break;
       case RCL_SYSTEM_TIME:

rclcpp/test/rclcpp/test_clock_conditional.cpp

@@ -136,7 +136,7 @@ INSTANTIATE_TEST_SUITE_P(
   ClockConditionalVariable,
   TestClockWakeup,
   ::testing::Values(
-    RCL_SYSTEM_TIME, RCL_ROS_TIME, RCL_STEADY_TIME
+    RCL_SYSTEM_TIME, RCL_ROS_TIME, RCL_STEADY_TIME, RCL_RAW_STEADY_TIME
 ));
 
 TEST_P(TestClockWakeup, wakeup_sleep) {

rclcpp/test/rclcpp/test_logging.cpp

@@ -190,41 +190,54 @@ TEST_F(TestLoggingMacros, test_throttle) {
     std::this_thread::sleep_for(50ms);
   }
   EXPECT_EQ(4u, g_log_calls);
+  rclcpp::Clock raw_steady_clock(RCL_RAW_STEADY_TIME);
+  for (uint64_t i = 0; i < 3; ++i) {
+    RCLCPP_DEBUG_THROTTLE(g_logger, raw_steady_clock, 10000, "Throttling");
+  }
+  EXPECT_EQ(5u, g_log_calls);
+  RCLCPP_DEBUG_SKIPFIRST_THROTTLE(g_logger, raw_steady_clock, 1, "Skip first throttling");
+  EXPECT_EQ(5u, g_log_calls);
+  for (uint64_t i = 0; i < 6; ++i) {
+    RCLCPP_DEBUG_THROTTLE(g_logger, raw_steady_clock, 100, "Throttling");
+    RCLCPP_DEBUG_SKIPFIRST_THROTTLE(g_logger, raw_steady_clock, 400, "Throttling");
+    std::this_thread::sleep_for(50ms);
+  }
+  EXPECT_EQ(8u, g_log_calls);
   rclcpp::Clock ros_clock(RCL_ROS_TIME);
   ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock.get_clock_handle()));
   RCLCPP_DEBUG_THROTTLE(g_logger, ros_clock, 10000, "Throttling");
   rcl_clock_t * clock = ros_clock.get_clock_handle();
   ASSERT_TRUE(clock);
-  EXPECT_EQ(4u, g_log_calls);
+  EXPECT_EQ(8u, g_log_calls);
   EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock, RCUTILS_MS_TO_NS(10)));
   for (uint64_t i = 0; i < 2; ++i) {
     RCLCPP_DEBUG_THROTTLE(g_logger, ros_clock, 10, "Throttling");
     if (i == 0) {
-      EXPECT_EQ(5u, g_log_calls);
+      EXPECT_EQ(9u, g_log_calls);
       rcl_time_point_value_t clock_ns = ros_clock.now().nanoseconds() + RCUTILS_MS_TO_NS(10);
       EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(clock, clock_ns));
     } else {
-      EXPECT_EQ(6u, g_log_calls);
+      EXPECT_EQ(10u, g_log_calls);
     }
   }
   DummyNode node;
   rcl_clock_t * node_clock = node.get_clock()->get_clock_handle();
   ASSERT_TRUE(node_clock);
   ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(node_clock));
-  EXPECT_EQ(6u, g_log_calls);
+  EXPECT_EQ(10u, g_log_calls);
   EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(node_clock, RCUTILS_MS_TO_NS(10)));
   for (uint64_t i = 0; i < 3; ++i) {
     RCLCPP_DEBUG_THROTTLE(g_logger, *node.get_clock(), 10, "Throttling");
     if (i == 0) {
-      EXPECT_EQ(7u, g_log_calls);
+      EXPECT_EQ(11u, g_log_calls);
       rcl_time_point_value_t clock_ns = node.get_clock()->now().nanoseconds() + RCUTILS_MS_TO_NS(5);
       EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(node_clock, clock_ns));
     } else if (i == 1) {
-      EXPECT_EQ(7u, g_log_calls);
+      EXPECT_EQ(11u, g_log_calls);
       rcl_time_point_value_t clock_ns = node.get_clock()->now().nanoseconds() + RCUTILS_MS_TO_NS(5);
       EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(node_clock, clock_ns));
     } else {
-      EXPECT_EQ(8u, g_log_calls);
+      EXPECT_EQ(12u, g_log_calls);
     }
   }
 }

rclcpp/test/rclcpp/test_node.cpp

@@ -165,6 +165,29 @@ TEST_F(TestNode, construction_and_destruction) {
         "my_node", "/ns",
         options), std::invalid_argument);
   }
+
+  {
+    rclcpp::NodeOptions options;
+    options.clock_type(RCL_RAW_STEADY_TIME);
+    ASSERT_NO_THROW(
+    {
+      const auto node = std::make_shared<rclcpp::Node>("my_node", "/ns", options);
+      EXPECT_EQ(RCL_RAW_STEADY_TIME, node->get_clock()->get_clock_type());
+    });
+  }
+
+  {
+    rclcpp::NodeOptions options;
+    options.parameter_overrides(
+    {
+      {"use_sim_time", true},
+    });
+    options.clock_type(RCL_RAW_STEADY_TIME);
+    ASSERT_THROW(
+      const auto node = std::make_shared<rclcpp::Node>(
+        "my_node", "/ns",
+        options), std::invalid_argument);
+  }
 }
 
 /*

rclcpp/test/rclcpp/test_node_options.cpp

@@ -382,4 +382,6 @@ TEST(TestNodeOptions, clock_type) {
   EXPECT_EQ(RCL_SYSTEM_TIME, options.clock_type());
   options.clock_type(RCL_STEADY_TIME);
   EXPECT_EQ(RCL_STEADY_TIME, options.clock_type());
+  options.clock_type(RCL_RAW_STEADY_TIME);
+  EXPECT_EQ(RCL_RAW_STEADY_TIME, options.clock_type());
 }

rclcpp/test/rclcpp/test_rate.cpp

@@ -146,6 +146,10 @@ TEST_F(TestRate, clock_types) {
     rclcpp::Rate rate(1.0, std::make_shared<rclcpp::Clock>(RCL_STEADY_TIME));
     EXPECT_EQ(RCL_STEADY_TIME, rate.get_type());
   }
+  {
+    rclcpp::Rate rate(1.0, std::make_shared<rclcpp::Clock>(RCL_RAW_STEADY_TIME));
+    EXPECT_EQ(RCL_RAW_STEADY_TIME, rate.get_type());
+  }
   {
     rclcpp::Rate rate(1.0, std::make_shared<rclcpp::Clock>(RCL_ROS_TIME));
     EXPECT_EQ(RCL_ROS_TIME, rate.get_type());

rclcpp/test/rclcpp/test_time.cpp

@@ -63,6 +63,9 @@ TEST_F(TestTime, clock_type_access) {
 
   rclcpp::Clock steady_clock(RCL_STEADY_TIME);
   EXPECT_EQ(RCL_STEADY_TIME, steady_clock.get_clock_type());
+
+  rclcpp::Clock raw_steady_clock(RCL_RAW_STEADY_TIME);
+  EXPECT_EQ(RCL_RAW_STEADY_TIME, raw_steady_clock.get_clock_type());
 }
 
 // Check that the clock may go out of the scope before the jump callback without leading in UB.
@@ -93,6 +96,11 @@ TEST_F(TestTime, time_sources) {
   Time steady_now = steady_clock.now();
   EXPECT_NE(0, steady_now.sec);
   EXPECT_NE(0u, steady_now.nanosec);
+
+  rclcpp::Clock raw_steady_clock(RCL_RAW_STEADY_TIME);
+  Time raw_steady_now = raw_steady_clock.now();
+  EXPECT_NE(0, raw_steady_now.sec);
+  EXPECT_NE(0u, raw_steady_now.nanosec);
 }
 
 static const int64_t HALF_SEC_IN_NS = RCUTILS_MS_TO_NS(500);
@@ -193,30 +201,47 @@ TEST_F(TestTime, operators) {
 
   rclcpp::Time system_time(0, 0, RCL_SYSTEM_TIME);
   rclcpp::Time steady_time(0, 0, RCL_STEADY_TIME);
+  rclcpp::Time raw_steady_time(0, 0, RCL_RAW_STEADY_TIME);
 
   EXPECT_ANY_THROW((void)(system_time == steady_time));
+  EXPECT_ANY_THROW((void)(system_time == raw_steady_time));
   EXPECT_ANY_THROW((void)(system_time != steady_time));
+  EXPECT_ANY_THROW((void)(system_time != raw_steady_time));
   EXPECT_ANY_THROW((void)(system_time <= steady_time));
+  EXPECT_ANY_THROW((void)(system_time <= raw_steady_time));
   EXPECT_ANY_THROW((void)(system_time >= steady_time));
+  EXPECT_ANY_THROW((void)(system_time >= raw_steady_time));
   EXPECT_ANY_THROW((void)(system_time < steady_time));
+  EXPECT_ANY_THROW((void)(system_time < raw_steady_time));
   EXPECT_ANY_THROW((void)(system_time > steady_time));
+  EXPECT_ANY_THROW((void)(system_time > raw_steady_time));
   EXPECT_ANY_THROW((void)(system_time - steady_time));
+  EXPECT_ANY_THROW((void)(system_time - raw_steady_time));
 
   rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
   rclcpp::Clock steady_clock(RCL_STEADY_TIME);
+  rclcpp::Clock raw_steady_clock(RCL_RAW_STEADY_TIME);
 
   rclcpp::Time now = system_clock.now();
   rclcpp::Time later = steady_clock.now();
+  rclcpp::Time raw_later = raw_steady_clock.now();
 
   EXPECT_ANY_THROW((void)(now == later));
+  EXPECT_ANY_THROW((void)(now == raw_later));
   EXPECT_ANY_THROW((void)(now != later));
+  EXPECT_ANY_THROW((void)(now != raw_later));
   EXPECT_ANY_THROW((void)(now <= later));
+  EXPECT_ANY_THROW((void)(now <= raw_later));
   EXPECT_ANY_THROW((void)(now >= later));
+  EXPECT_ANY_THROW((void)(now >= raw_later));
   EXPECT_ANY_THROW((void)(now < later));
+  EXPECT_ANY_THROW((void)(now < raw_later));
   EXPECT_ANY_THROW((void)(now > later));
+  EXPECT_ANY_THROW((void)(now > raw_later));
   EXPECT_ANY_THROW((void)(now - later));
+  EXPECT_ANY_THROW((void)(now - raw_later));
 
-  for (auto time_source : {RCL_ROS_TIME, RCL_SYSTEM_TIME, RCL_STEADY_TIME}) {
+  for (auto time_source : {RCL_ROS_TIME, RCL_SYSTEM_TIME, RCL_STEADY_TIME, RCL_RAW_STEADY_TIME}) {
     rclcpp::Time time = rclcpp::Time(0, 0, time_source);
     rclcpp::Time copy_constructor_time(time);
     rclcpp::Time assignment_op_time = rclcpp::Time(1, 0, time_source);
@@ -312,7 +337,7 @@ TEST_F(TestTime, seconds) {
 TEST_F(TestTime, test_max) {
   // Same clock types
   for (rcl_clock_type_t type = RCL_ROS_TIME;
-    type != RCL_STEADY_TIME; type = static_cast<rcl_clock_type_t>(type + 1))
+    type != RCL_RAW_STEADY_TIME; type = static_cast<rcl_clock_type_t>(type + 1))
   {
     const rclcpp::Time time_max = rclcpp::Time::max(type);
     const rclcpp::Time max_time(std::numeric_limits<int32_t>::max(), 999999999, type);
@@ -323,13 +348,22 @@ TEST_F(TestTime, test_max) {
   {
     const rclcpp::Time time_max = rclcpp::Time::max(RCL_ROS_TIME);
     const rclcpp::Time max_time(std::numeric_limits<int32_t>::max(), 999999999, RCL_STEADY_TIME);
+    const rclcpp::Time raw_max_time(
+      std::numeric_limits<int32_t>::max(), 999999999, RCL_RAW_STEADY_TIME);
     EXPECT_ANY_THROW((void)(time_max == max_time));
+    EXPECT_ANY_THROW((void)(time_max == raw_max_time));
     EXPECT_ANY_THROW((void)(time_max != max_time));
+    EXPECT_ANY_THROW((void)(time_max != raw_max_time));
     EXPECT_ANY_THROW((void)(time_max <= max_time));
+    EXPECT_ANY_THROW((void)(time_max <= raw_max_time));
     EXPECT_ANY_THROW((void)(time_max >= max_time));
+    EXPECT_ANY_THROW((void)(time_max >= raw_max_time));
     EXPECT_ANY_THROW((void)(time_max < max_time));
+    EXPECT_ANY_THROW((void)(time_max < raw_max_time));
     EXPECT_ANY_THROW((void)(time_max > max_time));
+    EXPECT_ANY_THROW((void)(time_max > raw_max_time));
     EXPECT_ANY_THROW((void)(time_max - max_time));
+    EXPECT_ANY_THROW((void)(time_max - raw_max_time));
   }
 }
 
@@ -433,6 +467,11 @@ TEST_F(TestClockSleep, bad_clock_type) {
   RCLCPP_EXPECT_THROW_EQ(
     clock.sleep_until(ros_until),
     std::runtime_error("until's clock type does not match this clock's type"));
+
+  rclcpp::Time raw_steady_until(54321, 0, RCL_RAW_STEADY_TIME);
+  RCLCPP_EXPECT_THROW_EQ(
+    clock.sleep_until(raw_steady_until),
+    std::runtime_error("until's clock type does not match this clock's type"));
 }
 
 TEST_F(TestClockSleep, sleep_until_invalid_context) {
@@ -493,13 +532,34 @@ TEST_F(TestClockSleep, sleep_until_basic_steady) {
   EXPECT_GE(steady_end - steady_start, std::chrono::milliseconds(milliseconds));
 }
 
+TEST_F(TestClockSleep, sleep_until_basic_raw_steady) {
+  const auto milliseconds = 300;
+  rclcpp::Clock clock(RCL_RAW_STEADY_TIME);
+  auto delay = rclcpp::Duration(0, RCUTILS_MS_TO_NS(milliseconds));
+  auto sleep_until = clock.now() + delay;
+
+  auto steady_start = std::chrono::steady_clock::now();
+  ASSERT_TRUE(clock.sleep_until(sleep_until));
+  auto steady_end = std::chrono::steady_clock::now();
+
+  EXPECT_GE(clock.now(), sleep_until);
+  EXPECT_GE(steady_end - steady_start, std::chrono::milliseconds(milliseconds));
+}
+
 TEST_F(TestClockSleep, sleep_until_steady_past_returns_immediately) {
   rclcpp::Clock clock(RCL_STEADY_TIME);
   auto until = clock.now() - rclcpp::Duration(1000, 0);
   // This should return immediately, other possible behavior might be sleep forever and timeout
   ASSERT_TRUE(clock.sleep_until(until));
 }
 
+TEST_F(TestClockSleep, sleep_until_raw_steady_past_returns_immediately) {
+  rclcpp::Clock clock(RCL_RAW_STEADY_TIME);
+  auto until = clock.now() - rclcpp::Duration(1000, 0);
+  // This should return immediately, other possible behavior might be sleep forever and timeout
+  ASSERT_TRUE(clock.sleep_until(until));
+}
+
 TEST_F(TestClockSleep, sleep_until_system_past_returns_immediately) {
   rclcpp::Clock clock(RCL_SYSTEM_TIME);
   auto until = clock.now() - rclcpp::Duration(1000, 0);
@@ -660,6 +720,25 @@ TEST_F(TestClockSleep, sleep_for_basic_system) {
   EXPECT_GE(end - start, std::chrono::milliseconds(milliseconds));
 }
 
+TEST_F(TestClockSleep, sleep_for_basic_raw_steady) {
+  const auto milliseconds = 300;
+  rclcpp::Clock clock(RCL_RAW_STEADY_TIME);
+  auto rel_time = rclcpp::Duration(0, RCUTILS_MS_TO_NS(milliseconds));
+
+  auto steady_start = std::chrono::steady_clock::now();
+  ASSERT_TRUE(clock.sleep_for(rel_time));
+  auto steady_end = std::chrono::steady_clock::now();
+
+  EXPECT_GE(steady_end - steady_start, std::chrono::milliseconds(milliseconds));
+}
+
+TEST_F(TestClockSleep, sleep_for_raw_steady_past_returns_immediately) {
+  rclcpp::Clock clock(RCL_RAW_STEADY_TIME);
+  auto rel_time = rclcpp::Duration(-1000, 0);
+  // This should return immediately
+  ASSERT_TRUE(clock.sleep_for(rel_time));
+}
+
 TEST_F(TestClockSleep, sleep_for_basic_steady) {
   const auto milliseconds = 300;
   rclcpp::Clock clock(RCL_STEADY_TIME);

rclcpp/test/rclcpp/test_time_source.cpp

@@ -299,6 +299,21 @@ TEST(TimeSource, invalid_clock_type_for_sim_time)
   rclcpp::shutdown();
 }
 
+TEST(TimeSource, invalid_raw_steady_clock_type_for_sim_time)
+{
+  rclcpp::init(0, nullptr);
+
+  rclcpp::NodeOptions options;
+  options.clock_type(RCL_RAW_STEADY_TIME);
+  auto node = std::make_shared<rclcpp::Node>("my_node", options);
+  EXPECT_FALSE(
+    node->set_parameter(
+      rclcpp::Parameter(
+        "use_sim_time", rclcpp::ParameterValue(
+          true))).successful);
+  rclcpp::shutdown();
+}
+
 TEST_F(TestTimeSource, clock) {
   rclcpp::TimeSource ts(node);
   auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);