Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix test_time_source test #639

Merged
merged 2 commits into from
Mar 13, 2019
Merged

Fix test_time_source test #639

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6748fd4
Fix flakey test
Feb 27, 2019
8db95ca
Fix lint and uncrustify issues
Mar 7, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
150 changes: 115 additions & 35 deletions rclcpp/test/test_time_source.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,16 +55,83 @@ class TestTimeSource : public ::testing::Test
rclcpp::Node::SharedPtr node;
};

void trigger_clock_changes(
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)
{
auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
rmw_qos_profile_default);
// 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()) {
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's important to do an exact match here. If you do clock->now().nanoseconds() >= end_time.count() then we'll trivially return whenever we're in the part of the test that runs before simulated time is active because clock->now().nanoseconds is a very large number before simulated time is turned on

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);

rclcpp::WallRate loop_rate(50);
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This loop_rate didn't do much to help the situation. The problem isn't that we need more wall time for the test to work. We need more calls to 'spin_once'

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);

// 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)
{
auto clock_pub = node->create_publisher<rosgraph_msgs::msg::Clock>("clock",
rmw_qos_profile_default);

for (int i = 0; i < 5; ++i) {
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not sure why this test is doing multiple publishes - most of the intermediate clock values (0, 1, 2, 3) are thrown away by the tests. Maybe it's another attempt at working around executor.spin_once not handling the action you think it's handling

if (!rclcpp::ok()) {
break; // Break for ctrl-c
Expand All @@ -73,12 +140,23 @@ void trigger_clock_changes(
msg->clock.sec = i;
msg->clock.nanosec = 1000;
clock_pub->publish(msg);
executor.spin_once(1000000ns);
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Another way to make the test work better was to call executor.spin_once ten times in a row here. Then you'd start seeing the expected time at the end of a test run

loop_rate.sleep();

// 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
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)
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);

Expand All @@ -90,10 +168,12 @@ void set_use_sim_time_parameter(rclcpp::Node::SharedPtr node, rclcpp::ParameterV
for (auto & result : set_parameters_results) {
EXPECT_TRUE(result.successful);
}
// SyncParametersClient returns when parameters have been set on the node_parameters interface,
// but it doesn't mean the on_parameter_event subscription in TimeSource has been called.
// Spin some to handle that subscription.
rclcpp::spin_some(node);

// Same as above - workaround for a little bit of asynchronus 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(TestTimeSource, detachUnattached) {
Expand Down Expand Up @@ -169,17 +249,17 @@ TEST_F(TestTimeSource, clock) {
ts.attachClock(ros_clock);
EXPECT_FALSE(ros_clock->ros_time_is_active());

trigger_clock_changes(node);
trigger_clock_changes(node, ros_clock, false);

// Even now that we've recieved a message, ROS time should still not be active since the
// parameter has not been explicitly set.
EXPECT_FALSE(ros_clock->ros_time_is_active());

// Activate ROS time.
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
EXPECT_TRUE(ros_clock->ros_time_is_active());

trigger_clock_changes(node);
trigger_clock_changes(node, ros_clock);

auto t_out = ros_clock->now();

Expand Down Expand Up @@ -236,7 +316,10 @@ TEST_F(TestTimeSource, callbacks) {
ts.attachClock(ros_clock);
EXPECT_FALSE(ros_clock->ros_time_is_active());

trigger_clock_changes(node);
// Last arg below is 'expect_time_update' Since ros_time is not active yet, we don't expect
// the simulated time to be updated by trigger_clock_changes. The method will pump messages
// anyway, but won't fail the test when the simulated time doesn't update
trigger_clock_changes(node, ros_clock, false);
auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);

Expand All @@ -245,10 +328,10 @@ TEST_F(TestTimeSource, callbacks) {
EXPECT_EQ(0, cbo.last_postcallback_id_);

// Activate ROS time.
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
EXPECT_TRUE(ros_clock->ros_time_is_active());

trigger_clock_changes(node);
trigger_clock_changes(node, ros_clock);

auto t_out = ros_clock->now();

Expand All @@ -266,7 +349,7 @@ TEST_F(TestTimeSource, callbacks) {
std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 2),
jump_threshold);

trigger_clock_changes(node);
trigger_clock_changes(node, ros_clock);

EXPECT_EQ(2, cbo.last_precallback_id_);
EXPECT_EQ(2, cbo.last_postcallback_id_);
Expand All @@ -285,7 +368,7 @@ TEST_F(TestTimeSource, callbacks) {
std::function<void(rcl_time_jump_t)>(),
jump_threshold);

trigger_clock_changes(node);
trigger_clock_changes(node, ros_clock);
EXPECT_EQ(3, cbo.last_precallback_id_);
EXPECT_EQ(2, cbo.last_postcallback_id_);

Expand All @@ -295,7 +378,7 @@ TEST_F(TestTimeSource, callbacks) {
std::bind(&CallbackObject::post_callback, &cbo, std::placeholders::_1, 4),
jump_threshold);

trigger_clock_changes(node);
trigger_clock_changes(node, ros_clock);
EXPECT_EQ(3, cbo.last_precallback_id_);
EXPECT_EQ(4, cbo.last_postcallback_id_);
}
Expand Down Expand Up @@ -330,10 +413,10 @@ TEST_F(TestTimeSource, callback_handler_erasure) {
EXPECT_EQ(0, cbo.last_postcallback_id_);

// Activate ROS time.
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
EXPECT_TRUE(ros_clock->ros_time_is_active());

trigger_clock_changes(node);
trigger_clock_changes(node, ros_clock);

auto t_low = rclcpp::Time(1, 0, RCL_ROS_TIME);
auto t_high = rclcpp::Time(10, 100000, RCL_ROS_TIME);
Expand All @@ -357,7 +440,7 @@ TEST_F(TestTimeSource, callback_handler_erasure) {
// Remove the last callback in the vector
callback_handler2.reset();

trigger_clock_changes(node);
trigger_clock_changes(node, ros_clock);

EXPECT_EQ(2, cbo.last_precallback_id_);
EXPECT_EQ(2, cbo.last_postcallback_id_);
Expand All @@ -371,7 +454,6 @@ TEST_F(TestTimeSource, callback_handler_erasure) {
EXPECT_GT(t_high.nanoseconds(), t_out.nanoseconds());
}
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you printed out the value of t_out right here, you'd see that it was usually 1s + 1000ns. That's good enough to be between t_low and t_high, but it's not what you'd expect if everything was working correctly. It should be 4s + 1000ns at the end of the test



TEST_F(TestTimeSource, parameter_activation) {
rclcpp::TimeSource ts(node);
auto ros_clock = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
Expand All @@ -380,27 +462,25 @@ TEST_F(TestTimeSource, parameter_activation) {
ts.attachClock(ros_clock);
EXPECT_FALSE(ros_clock->ros_time_is_active());

set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
EXPECT_TRUE(ros_clock->ros_time_is_active());

set_use_sim_time_parameter(
node, rclcpp::ParameterValue());
set_use_sim_time_parameter(node, rclcpp::ParameterValue(), ros_clock);
EXPECT_TRUE(ros_clock->ros_time_is_active());

set_use_sim_time_parameter(node, rclcpp::ParameterValue(false));
set_use_sim_time_parameter(node, rclcpp::ParameterValue(false), ros_clock);
EXPECT_FALSE(ros_clock->ros_time_is_active());

set_use_sim_time_parameter(
node, rclcpp::ParameterValue());
set_use_sim_time_parameter(node, rclcpp::ParameterValue(), ros_clock);
EXPECT_FALSE(ros_clock->ros_time_is_active());

// If the use_sim_time parameter is not explicitly set to True, this clock's use of sim time
// should not be affected by the presence of a clock publisher.
trigger_clock_changes(node);
trigger_clock_changes(node, ros_clock, false);
EXPECT_FALSE(ros_clock->ros_time_is_active());
set_use_sim_time_parameter(node, rclcpp::ParameterValue(false));
set_use_sim_time_parameter(node, rclcpp::ParameterValue(false), ros_clock);
EXPECT_FALSE(ros_clock->ros_time_is_active());
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
EXPECT_TRUE(ros_clock->ros_time_is_active());
}

Expand All @@ -425,7 +505,7 @@ TEST_F(TestTimeSource, no_pre_jump_callback) {
ts.attachClock(ros_clock);

// Activate ROS time
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true));
set_use_sim_time_parameter(node, rclcpp::ParameterValue(true), ros_clock);
ASSERT_TRUE(ros_clock->ros_time_is_active());

EXPECT_EQ(0, cbo.last_precallback_id_);
Expand Down