diff --git a/rclcpp/include/rclcpp/timer.hpp b/rclcpp/include/rclcpp/timer.hpp index 0ed62007d4..e5f4b4b3cf 100644 --- a/rclcpp/include/rclcpp/timer.hpp +++ b/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: diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 46c1f09981..97de3ba92c 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -14,6 +14,7 @@ #include "rclcpp/clock.hpp" +#include #include #include #include @@ -120,6 +121,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 +484,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: diff --git a/rclcpp/test/rclcpp/test_clock_conditional.cpp b/rclcpp/test/rclcpp/test_clock_conditional.cpp index e924e212d7..a1a46545f7 100644 --- a/rclcpp/test/rclcpp/test_clock_conditional.cpp +++ b/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) { diff --git a/rclcpp/test/rclcpp/test_logging.cpp b/rclcpp/test/rclcpp/test_logging.cpp index 8e2214f8e6..26a1e9dec7 100644 --- a/rclcpp/test/rclcpp/test_logging.cpp +++ b/rclcpp/test/rclcpp/test_logging.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include #include @@ -190,41 +191,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); } } } diff --git a/rclcpp/test/rclcpp/test_node.cpp b/rclcpp/test/rclcpp/test_node.cpp index dc8eedb64e..462fba4c71 100644 --- a/rclcpp/test/rclcpp/test_node.cpp +++ b/rclcpp/test/rclcpp/test_node.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -165,6 +166,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("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( + "my_node", "/ns", + options), std::invalid_argument); + } } /* diff --git a/rclcpp/test/rclcpp/test_node_options.cpp b/rclcpp/test/rclcpp/test_node_options.cpp index 07a522f497..62eaf1d772 100644 --- a/rclcpp/test/rclcpp/test_node_options.cpp +++ b/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()); } diff --git a/rclcpp/test/rclcpp/test_rate.cpp b/rclcpp/test/rclcpp/test_rate.cpp index 86d22cbd94..f03e60f847 100644 --- a/rclcpp/test/rclcpp/test_rate.cpp +++ b/rclcpp/test/rclcpp/test_rate.cpp @@ -146,6 +146,10 @@ TEST_F(TestRate, clock_types) { rclcpp::Rate rate(1.0, std::make_shared(RCL_STEADY_TIME)); EXPECT_EQ(RCL_STEADY_TIME, rate.get_type()); } + { + rclcpp::Rate rate(1.0, std::make_shared(RCL_RAW_STEADY_TIME)); + EXPECT_EQ(RCL_RAW_STEADY_TIME, rate.get_type()); + } { rclcpp::Rate rate(1.0, std::make_shared(RCL_ROS_TIME)); EXPECT_EQ(RCL_ROS_TIME, rate.get_type()); diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index 8406de4efb..851bcc55ec 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/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(type + 1)) + type != RCL_RAW_STEADY_TIME; type = static_cast(type + 1)) { const rclcpp::Time time_max = rclcpp::Time::max(type); const rclcpp::Time max_time(std::numeric_limits::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::max(), 999999999, RCL_STEADY_TIME); + const rclcpp::Time raw_max_time( + std::numeric_limits::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,6 +532,20 @@ 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); @@ -500,6 +553,13 @@ TEST_F(TestClockSleep, sleep_until_steady_past_returns_immediately) { 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); diff --git a/rclcpp/test/rclcpp/test_time_source.cpp b/rclcpp/test/rclcpp/test_time_source.cpp index cb799cbae0..a7daf0df19 100644 --- a/rclcpp/test/rclcpp/test_time_source.cpp +++ b/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("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(RCL_ROS_TIME);