From 6a54627c9f5f902758a7ff9c265b03f5c312297a Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 1 Nov 2022 15:50:00 -0700 Subject: [PATCH] Refine tests and update docstrings Signed-off-by: methylDragon --- rclcpp/include/rclcpp/clock.hpp | 13 +-- rclcpp/src/rclcpp/clock.cpp | 19 ++-- rclcpp/test/rclcpp/test_time.cpp | 175 ++----------------------------- 3 files changed, 27 insertions(+), 180 deletions(-) diff --git a/rclcpp/include/rclcpp/clock.hpp b/rclcpp/include/rclcpp/clock.hpp index de6eadd3f8..9d3d585753 100644 --- a/rclcpp/include/rclcpp/clock.hpp +++ b/rclcpp/include/rclcpp/clock.hpp @@ -140,8 +140,8 @@ class Clock /** * Check if the clock is valid. * - * An invalid clock is either a clock that is RCL_CLOCK_UNINITIALIZED or is an RCL_ROS_TIME - * clock that is using ros time (ros_time_is_active()) but with time 0. + * An invalid clock is either a clock that is RCL_CLOCK_UNINITIALIZED or a clock with zero-time. + * In other words, an uninitialized clock. * * \return true if clock was or became valid */ @@ -152,15 +152,12 @@ class Clock /** * Wait for clock to become valid. * - * \param timeout the maximum time to wait for. - * \param wait_tick_ns the time to wait between each iteration of the wait loop (in nanoseconds). + * \param context the context to wait in. * \return true if clock was or became valid */ RCLCPP_PUBLIC bool - wait_for_valid( - Context::SharedPtr context = contexts::get_global_default_context(), - Duration wait_tick_ns = Duration(0, 1e7)); + wait_for_valid(Context::SharedPtr context = contexts::get_global_default_context()); /** * Wait for clock to become valid, with timeout. @@ -177,7 +174,7 @@ class Clock wait_for_valid( const rclcpp::Duration & timeout, Context::SharedPtr context = contexts::get_global_default_context(), - Duration wait_tick_ns = Duration(0, 1e7)); + Duration wait_tick_ns = Duration(0, static_cast(1e7))); /** * Returns the clock of the type `RCL_ROS_TIME` is active. diff --git a/rclcpp/src/rclcpp/clock.cpp b/rclcpp/src/rclcpp/clock.cpp index 8a096a8881..70fd248be8 100644 --- a/rclcpp/src/rclcpp/clock.cpp +++ b/rclcpp/src/rclcpp/clock.cpp @@ -198,9 +198,15 @@ Clock::is_valid() } bool -Clock::wait_for_valid(Context::SharedPtr context, Duration wait_tick_ns) +Clock::wait_for_valid(Context::SharedPtr context) { - return wait_for_valid(Duration(0, 0), context, wait_tick_ns); + if (get_clock_type() == RCL_CLOCK_UNINITIALIZED) { + throw std::runtime_error( + "clock cannot be waited on as its clock_type is RCL_CLOCK_UNINITIALIZED"); + } + + // Wait until the first valid time + return sleep_until(rclcpp::Time(0, 1, get_clock_type()), context); } bool @@ -221,17 +227,14 @@ Clock::wait_for_valid( Clock timeout_clock = Clock(RCL_SYSTEM_TIME); Time start = timeout_clock.now(); - while (!is_valid() && rclcpp::ok(context)) { + while (!is_valid() && context->is_valid()) { timeout_clock.sleep_for(Duration(wait_tick_ns)); - if (timeout > rclcpp::Duration(0, 0) && (timeout_clock.now() - start > timeout)) { + if (timeout_clock.now() - start > timeout) { return false; } } - if (!rclcpp::ok(context)) { - return false; - } - return true; + return context->is_valid(); } diff --git a/rclcpp/test/rclcpp/test_time.cpp b/rclcpp/test/rclcpp/test_time.cpp index 202b85e706..4f427d26c3 100644 --- a/rclcpp/test/rclcpp/test_time.cpp +++ b/rclcpp/test/rclcpp/test_time.cpp @@ -853,204 +853,51 @@ TEST_F(TestClockSleep, sleep_for_basic_ros) { class TestClockValid : public ::testing::Test { protected: - static void SetUpTestCase() - { - rclcpp::init(0, nullptr); - } - - static void TearDownTestCase() - { - rclcpp::shutdown(); - } - void SetUp() { - node = std::make_shared("my_node"); + rclcpp::init(0, nullptr); } void TearDown() { - node.reset(); + rclcpp::shutdown(); } - - rclcpp::Node::SharedPtr node; }; -void spin_until_time( - rclcpp::Clock::SharedPtr clock, - rclcpp::Node::SharedPtr node, - std::chrono::nanoseconds end_time, - bool expect_time_update) -{ - // Call spin_once on the node until either: - // 1) We see the ros_clock's simulated time change to the expected end_time - // -or- - // 2) 1 second has elapsed in the real world - // If 'expect_time_update' is True, and we timed out waiting for simulated time to - // update, we'll have the test fail - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - - auto start = std::chrono::system_clock::now(); - while (std::chrono::system_clock::now() < (start + 1s)) { - if (!rclcpp::ok()) { - break; // Break for ctrl-c - } - - executor.spin_once(10ms); - - if (clock->now().nanoseconds() >= end_time.count()) { - return; - } - } - - if (expect_time_update) { - // If we were expecting ROS clock->now to be updated and we didn't take the early return from - // the loop up above, that's a failure - ASSERT_TRUE(false) << "Timed out waiting for ROS time to update"; - } -} - -void spin_until_ros_time_updated( - rclcpp::Clock::SharedPtr clock, - rclcpp::Node::SharedPtr node, - rclcpp::ParameterValue value) -{ - // Similar to above: Call spin_once until we see the clock's ros_time_is_active method - // match the ParameterValue - // Unlike spin_until_time, there aren't any test cases where we don't expect the value to - // update. In the event that the ParameterValue is not set, we'll pump messages for a full second - // but we don't cause the test to fail - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - - auto start = std::chrono::system_clock::now(); - while (std::chrono::system_clock::now() < (start + 2s)) { - if (!rclcpp::ok()) { - break; // Break for ctrl-c - } - - executor.spin_once(10ms); - - // In the case where we didn't intend to change the parameter, we'll still pump - if (value.get_type() == rclcpp::ParameterType::PARAMETER_NOT_SET) { - continue; - } - - if (clock->ros_time_is_active() == value.get()) { - return; - } - } -} - -void trigger_clock_changes( - rclcpp::Node::SharedPtr node, - std::shared_ptr clock, - bool expect_time_update = true, - bool zero = false) -{ - auto clock_pub = node->create_publisher("clock", 10); - - for (int i = 0; i < 5; ++i) { - if (!rclcpp::ok()) { - break; // Break for ctrl-c - } - rosgraph_msgs::msg::Clock msg; - - if (zero) { - msg.clock.sec = 0; - msg.clock.nanosec = 0; - } else { - msg.clock.sec = i; - msg.clock.nanosec = 1000; - } - clock_pub->publish(msg); - - // workaround. Long-term, there can be a more elegant fix where we hook a future up - // to a clock change callback and spin until future complete, but that's an upstream - // change - if (zero) { - spin_until_time( - clock, - node, - std::chrono::nanoseconds(0), - expect_time_update - ); - } else { - spin_until_time( - clock, - node, - std::chrono::seconds(i) + std::chrono::nanoseconds(1000), - expect_time_update - ); - } - } -} - -void set_use_sim_time_parameter( - rclcpp::Node::SharedPtr node, - rclcpp::ParameterValue value, - rclcpp::Clock::SharedPtr clock) -{ - auto parameters_client = std::make_shared(node); - - using namespace std::chrono_literals; - EXPECT_TRUE(parameters_client->wait_for_service(2s)); - auto set_parameters_results = parameters_client->set_parameters( - { - rclcpp::Parameter("use_sim_time", value) - }); - for (auto & result : set_parameters_results) { - EXPECT_TRUE(result.successful); - } - - // Same as above - workaround for a little bit of asynchronous behavior. The sim_time paramater - // is set synchronously, but the way the ros clock gets notified involves a pub/sub that happens - // AFTER the synchronous notification that the parameter was set. This may also get fixed - // upstream - spin_until_ros_time_updated(clock, node, value); -} - TEST_F(TestClockValid, validity) { rclcpp::Clock ros_clock(RCL_ROS_TIME); EXPECT_TRUE(ros_clock.is_valid()); EXPECT_TRUE(ros_clock.wait_for_valid()); - EXPECT_TRUE(ros_clock.wait_for_valid(rclcpp::Duration(0, 1e7))); + EXPECT_TRUE(ros_clock.wait_for_valid(rclcpp::Duration(0, static_cast(1e7)))); rclcpp::Clock system_clock(RCL_SYSTEM_TIME); EXPECT_TRUE(system_clock.is_valid()); EXPECT_TRUE(system_clock.wait_for_valid()); - EXPECT_TRUE(system_clock.wait_for_valid(rclcpp::Duration(0, 1e7))); + EXPECT_TRUE(system_clock.wait_for_valid(rclcpp::Duration(0, static_cast(1e7)))); rclcpp::Clock steady_clock(RCL_STEADY_TIME); EXPECT_TRUE(steady_clock.is_valid()); EXPECT_TRUE(steady_clock.wait_for_valid()); - EXPECT_TRUE(steady_clock.wait_for_valid(rclcpp::Duration(0, 1e7))); + EXPECT_TRUE(steady_clock.wait_for_valid(rclcpp::Duration(0, static_cast(1e7)))); rclcpp::Clock uninit_clock(RCL_CLOCK_UNINITIALIZED); EXPECT_FALSE(uninit_clock.is_valid()); RCLCPP_EXPECT_THROW_EQ( - uninit_clock.wait_for_valid(rclcpp::Duration(0, 1e7)), + uninit_clock.wait_for_valid(rclcpp::Duration(0, static_cast(1e7))), std::runtime_error("clock cannot be waited on as its clock_type is RCL_CLOCK_UNINITIALIZED")); } TEST_F(TestClockValid, invalid_timeout) { - // We need to borrow some logic from the time_source tests to set clock time to (0, 0) - rclcpp::TimeSource ts(node); auto ros_clock = std::make_shared(RCL_ROS_TIME); - ts.attachClock(ros_clock); - trigger_clock_changes(node, ros_clock, false); - EXPECT_FALSE(ros_clock->ros_time_is_active()); + auto ros_clock_handle = ros_clock->get_clock_handle(); - set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock); + EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(ros_clock_handle)); EXPECT_TRUE(ros_clock->ros_time_is_active()); - trigger_clock_changes(node, ros_clock, true, true); // Force to time zero + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(ros_clock_handle, 0)); EXPECT_FALSE(ros_clock->is_valid()); - EXPECT_FALSE(ros_clock->wait_for_valid(rclcpp::Duration(0, 1e7))); + EXPECT_FALSE(ros_clock->wait_for_valid(rclcpp::Duration(0, static_cast(1e7)))); std::thread t([]() { std::this_thread::sleep_for(std::chrono::seconds(1)); @@ -1058,7 +905,7 @@ TEST_F(TestClockValid, invalid_timeout) { }); // Test rclcpp shutdown escape hatch (otherwise this waits indefinitely) - EXPECT_FALSE(ros_clock->wait_for_valid(rclcpp::Duration(0, 0))); + EXPECT_FALSE(ros_clock->wait_for_valid()); t.join(); }