Skip to content

Commit

Permalink
Refine tests and update docstrings
Browse files Browse the repository at this point in the history 
Signed-off-by: methylDragon <methylDragon@gmail.com>
  • Loading branch information
@methylDragon
methylDragon committed Nov 2, 2022
1 parent b6d2c52 commit 23271a6
Show file tree
Hide file tree
Showing 3 changed files with 21 additions and 174 deletions.
11 changes: 4 additions & 7 deletions rclcpp/include/rclcpp/clock.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
*/
Expand All @@ -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.
Expand Down
19 changes: 11 additions & 8 deletions rclcpp/src/rclcpp/clock.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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();
}


Expand Down
165 changes: 6 additions & 159 deletions rclcpp/test/rclcpp/test_time.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -853,166 +853,17 @@ 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<rclcpp::Node>("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<bool>()) {
return;
}
}
}

void trigger_clock_changes(
rclcpp::Node::SharedPtr node,
std::shared_ptr<rclcpp::Clock> clock,
bool expect_time_update = true,
bool zero = false)
{
auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("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<rclcpp::SyncParametersClient>(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());
Expand All @@ -1037,17 +888,13 @@ TEST_F(TestClockValid, validity) {
}

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<rclcpp::Clock>(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)));
Expand All @@ -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();
}

0 comments on commit 23271a6

Please sign in to comment.