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.

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

Add tests for unsupported qos checks on initialize #5

Merged
merged 4 commits into from
Apr 3, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
12 changes: 8 additions & 4 deletions rcl/src/rcl/event.c
Original file line number Diff line number Diff line change
Expand Up @@ -82,8 +82,10 @@ rcl_publisher_event_init(
return RCL_RET_INVALID_ARGUMENT;
}

return rmw_publisher_event_init(&event->impl->rmw_handle,
publisher->impl->rmw_handle, rmw_event_type);
return rmw_publisher_event_init(
&event->impl->rmw_handle,
publisher->impl->rmw_handle,
rmw_event_type);
}

rcl_ret_t
Expand Down Expand Up @@ -120,8 +122,10 @@ rcl_subscription_event_init(
return RCL_RET_INVALID_ARGUMENT;
}

return rmw_subscription_event_init(&event->impl->rmw_handle,
subscription->impl->rmw_handle, rmw_event_type);
return rmw_subscription_event_init(
&event->impl->rmw_handle,
subscription->impl->rmw_handle,
rmw_event_type);
}

rcl_ret_t
Expand Down
2 changes: 1 addition & 1 deletion rcl/test/rcl/test_count_matched.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -194,7 +194,7 @@ TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION),

rcl_publisher_t pub = rcl_get_zero_initialized_publisher();

rcl_publisher_options_t pub_ops;
rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options();
pub_ops.qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
pub_ops.qos.depth = 10;
pub_ops.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
Expand Down
191 changes: 156 additions & 35 deletions rcl/test/rcl/test_events.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,12 @@

#include "osrf_testing_tools_cpp/scope_exit.hpp"

constexpr int DISCOVERY_WAIT_TIME_IN_MS = 1000;
constexpr int LIVELINESS_LEASE_DURATION_IN_MS = 1000;
constexpr int DEADLINE_PERIOD_IN_MS = 1000;
using std::chrono::milliseconds;
using std::chrono::seconds;

constexpr seconds DISCOVERY_WAIT_TIME_IN_S(1);
constexpr seconds LIVELINESS_LEASE_DURATION_IN_S(1);
constexpr seconds DEADLINE_PERIOD_IN_S(1);

#ifdef RMW_IMPLEMENTATION
# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
Expand All @@ -38,11 +41,17 @@ constexpr int DEADLINE_PERIOD_IN_MS = 1000;
# define CLASSNAME(NAME, SUFFIX) NAME
#endif

class CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test
class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test
{
public:
void SetUp()
{
const bool is_opensplice =
std::string(rmw_get_implementation_identifier()).find("rmw_opensplice") == 0;
const bool is_fastrtps =
std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") == 0;

is_unsupported = is_fastrtps || is_opensplice;
rcl_ret_t ret;
{
rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
Expand All @@ -62,26 +71,57 @@ class CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test
rcl_node_options_t node_options = rcl_node_get_default_options();
ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Primitives);
}

void SetUpPubSub(
const rcl_publisher_event_type_t pub_event_type,
const rcl_subscription_event_type_t sub_event_type)
rcl_ret_t SetupPublisher(
const rmw_time_t & deadline,
const rmw_time_t & lifespan,
const rmw_time_t & liveliness_lease_duration,
const rmw_qos_liveliness_policy_t liveliness_policy)
{
// init publisher
publisher = rcl_get_zero_initialized_publisher();
rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
publisher_options.qos.deadline = deadline;
publisher_options.qos.lifespan = lifespan;
publisher_options.qos.liveliness = liveliness_policy;
publisher_options.qos.liveliness_lease_duration = liveliness_lease_duration;
return rcl_publisher_init(&publisher, this->node_ptr, this->ts, this->topic,
&publisher_options);
}

rcl_ret_t SetupSubscriber(
const rmw_time_t & deadline,
const rmw_time_t & lifespan,
const rmw_time_t & liveliness_lease_duration,
const rmw_qos_liveliness_policy_t liveliness_policy)
{
// init publisher
subscription = rcl_get_zero_initialized_subscription();
rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options();
subscription_options.qos.deadline = deadline;
subscription_options.qos.lifespan = lifespan;
subscription_options.qos.liveliness = liveliness_policy;
subscription_options.qos.liveliness_lease_duration = liveliness_lease_duration;

return rcl_subscription_init(&subscription, this->node_ptr, this->ts, this->topic,
&subscription_options);
}

void SetupPublisherAndSubscriber(
const rcl_publisher_event_type_t & pub_event_type,
const rcl_subscription_event_type_t & sub_event_type)
{
rcl_ret_t ret;

const char * topic = "rcl_test_publisher_subscription_events";
const rosidl_message_type_support_t * ts =
ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Primitives);
rmw_time_t lifespan {0, 0};
rmw_time_t deadline {DEADLINE_PERIOD_IN_S.count(), 0};
rmw_time_t lease_duration {LIVELINESS_LEASE_DURATION_IN_S.count(), 0};
rmw_qos_liveliness_policy_t liveliness_policy = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;

// init publisher
publisher = rcl_get_zero_initialized_publisher();
rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options();
publisher_options.qos.deadline = rmw_time_t {DEADLINE_PERIOD_IN_MS / 1000, 0};
publisher_options.qos.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
publisher_options.qos.liveliness_lease_duration =
rmw_time_t {LIVELINESS_LEASE_DURATION_IN_MS / 1000, 0};
ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options);
ret = SetupPublisher(deadline, lifespan, lease_duration, liveliness_policy);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;

// init publisher events
Expand All @@ -90,13 +130,7 @@ class CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;

// init subscription
subscription = rcl_get_zero_initialized_subscription();
rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options();
subscription_options.qos.deadline = rmw_time_t {DEADLINE_PERIOD_IN_MS / 1000, 0};
subscription_options.qos.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
subscription_options.qos.liveliness_lease_duration =
rmw_time_t {LIVELINESS_LEASE_DURATION_IN_MS / 1000, 0};
ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options);
ret = SetupSubscriber(deadline, lifespan, lease_duration, liveliness_policy);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;

// init subscription event
Expand Down Expand Up @@ -146,6 +180,9 @@ class CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION) : public ::testing::Test
rcl_event_t publisher_event;
rcl_subscription_t subscription;
rcl_event_t subscription_event;
bool is_unsupported;
const char * topic = "rcl_test_publisher_subscription_events";
const rosidl_message_type_support_t * ts;
};

bool
Expand All @@ -169,7 +206,7 @@ wait_for_msgs_and_events(

rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
rcl_ret_t ret = rcl_wait_set_init(&wait_set, num_subscriptions, 0, 0, 0, 0, num_events,
context, rcl_get_default_allocator());
context, rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
Expand Down Expand Up @@ -220,19 +257,95 @@ wait_for_msgs_and_events(
return subscription_event_ready || publisher_event_ready;
}

TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_unsupported_lifespan) {
if (is_unsupported) {
rmw_time_t deadline {0, 0};
rmw_time_t lifespan {1, 0};
rmw_time_t lease_duration {1, 0};
rmw_qos_liveliness_policy_t liveliness_policy = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
EXPECT_EQ(RMW_RET_ERROR,
Copy link

Choose a reason for hiding this comment

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

I would expect all of these to be receiving back RMW_RET_UNSUPPORTED if they're failing because it is unsupported. Are we masking that error code at some layer?

https://github.com/aws-robotics/rmw/blob/qos_reviewed/rmw/include/rmw/ret_types.h#L31

Copy link
Author

Choose a reason for hiding this comment

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

That will not be possible, since the init function does not return RMW_RET_T, but instead a pointer: nullptr if unable to initialize, or a filled ptr.

Copy link
Author

Choose a reason for hiding this comment

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

We could always change the function prototype 🗡️

Copy link

Choose a reason for hiding this comment

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

Yeah, we can add an issue to the backlog to add a return error code from this function. It's certainly not a blocker for this PR.

SetupSubscriber(deadline, lifespan, lease_duration,
liveliness_policy)) << "Initialized subscriber lifespan when unsupported";
EXPECT_EQ(RMW_RET_ERROR,
SetupPublisher(deadline, lifespan, lease_duration,
liveliness_policy)) << "Initialized publisher lifespan when unsupported";

lifespan = {0, 1};
EXPECT_EQ(RMW_RET_ERROR,
SetupSubscriber(deadline, lifespan, lease_duration,
liveliness_policy)) << "Initialized subscriber lifespan when unsupported";
EXPECT_EQ(RMW_RET_ERROR,
SetupPublisher(deadline, lifespan, lease_duration,
liveliness_policy)) << "Initialized publisher lifespan when unsupported";
}
}

TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_unsupported_liveliness) {
if (is_unsupported) {
rmw_time_t deadline {0, 0};
rmw_time_t lifespan {0, 0};
rmw_time_t lease_duration {0, 0};
rmw_qos_liveliness_policy_t liveliness_policy = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE;
EXPECT_EQ(RMW_RET_ERROR,
SetupSubscriber(deadline, lifespan, lease_duration,
liveliness_policy)) <<
"Initialized subscriber RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE when unsupported";
EXPECT_EQ(RMW_RET_ERROR,
SetupPublisher(deadline, lifespan, lease_duration,
liveliness_policy)) <<
"Initialized publisher RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE when unsupported";

liveliness_policy = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC;
EXPECT_EQ(RMW_RET_ERROR,
SetupSubscriber(deadline, lifespan, lease_duration,
liveliness_policy)) <<
"Initialized subscriber RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC when unsupported";
EXPECT_EQ(RMW_RET_ERROR,
SetupPublisher(deadline, lifespan, lease_duration,
liveliness_policy)) <<
"Initialized publisher RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC when unsupported";
}
}

TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_unsupported_unsupported_deadline) {
if (is_unsupported) {
rmw_time_t deadline;
rmw_time_t lifespan {0, 0};
rmw_time_t lease_duration {0, 0};
rmw_qos_liveliness_policy_t liveliness_policy = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
EXPECT_EQ(RMW_RET_ERROR,
SetupSubscriber(deadline, lifespan, lease_duration,
liveliness_policy)) << "Initialized subscriber deadline when unsupported";
EXPECT_EQ(RMW_RET_ERROR,
SetupPublisher(deadline, lifespan, lease_duration,
liveliness_policy)) << "Initialized publisher deadline when unsupported";

deadline = {0, 1};
EXPECT_EQ(RMW_RET_ERROR,
SetupSubscriber(deadline, lifespan, lease_duration,
liveliness_policy)) << "Initialized subscriber deadline when unsupported";
EXPECT_EQ(RMW_RET_ERROR,
SetupPublisher(deadline, lifespan, lease_duration,
liveliness_policy)) << "Initialized publisher deadline when unsupported";
}
}

/*
* Basic test of publisher and subscriber liveliness events, with publisher killed
*/
TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_liveliness_kill_pub)
{
// initialize publisher and subscriber
SetUpPubSub(RCL_PUBLISHER_LIVELINESS_LOST, RCL_SUBSCRIPTION_LIVELINESS_CHANGED);
if (is_unsupported) {
return;
}

SetupPublisherAndSubscriber(RCL_PUBLISHER_LIVELINESS_LOST, RCL_SUBSCRIPTION_LIVELINESS_CHANGED);

// wait for discovery
// TODO(wjwwood): add logic to wait for the connection to be established
// probably using the count_subscriptions busy wait mechanism
// until then we will sleep for a short period of time
std::this_thread::sleep_for(std::chrono::milliseconds(DISCOVERY_WAIT_TIME_IN_MS));
std::this_thread::sleep_for(DISCOVERY_WAIT_TIME_IN_S);

rcl_ret_t ret;

Expand All @@ -254,7 +367,7 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_liveliness_k
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;

// wait for lease duration to expire
std::this_thread::sleep_for(std::chrono::milliseconds(LIVELINESS_LEASE_DURATION_IN_MS + 500));
std::this_thread::sleep_for(LIVELINESS_LEASE_DURATION_IN_S + milliseconds(500));

// wait for events
bool msg_ready, subscription_event_ready, publisher_event_ready;
Expand Down Expand Up @@ -301,14 +414,18 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_liveliness_k
*/
TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_deadline_missed)
{
// initialize publisher and subscriber
SetUpPubSub(RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
if (is_unsupported) {
return;
}

SetupPublisherAndSubscriber(RCL_PUBLISHER_OFFERED_DEADLINE_MISSED,
RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);

// wait for discovery. also adds delay to publishing of message
// TODO(wjwwood): add logic to wait for the connection to be established
// probably using the count_subscriptions busy wait mechanism
// until then we will sleep for a short period of time
std::this_thread::sleep_for(std::chrono::milliseconds(DEADLINE_PERIOD_IN_MS + 500));
std::this_thread::sleep_for(DEADLINE_PERIOD_IN_S + milliseconds(500));

rcl_ret_t ret;

Expand Down Expand Up @@ -370,14 +487,18 @@ TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_deadline_mis
*/
TEST_F(CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION), test_pubsub_no_deadline_missed)
{
// initialize publisher and subscriber
SetUpPubSub(RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);
if (is_unsupported) {
return;
}

SetupPublisherAndSubscriber(RCL_PUBLISHER_OFFERED_DEADLINE_MISSED,
RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED);

// wait for discovery. also adds delay to publishing of message
// TODO(wjwwood): add logic to wait for the connection to be established
// probably using the count_subscriptions busy wait mechanism
// until then we will sleep for a short period of time
std::this_thread::sleep_for(std::chrono::milliseconds(DEADLINE_PERIOD_IN_MS - 500));
std::this_thread::sleep_for(DEADLINE_PERIOD_IN_S - milliseconds(500));

rcl_ret_t ret;

Expand Down
2 changes: 1 addition & 1 deletion rcl/test/rcl/test_timer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -460,7 +460,7 @@ TEST_F(TestTimerFixture, test_ros_time_wakes_wait) {
std::thread wait_thr([&](void) {
rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr,
rcl_get_default_allocator());
rcl_get_default_allocator());
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;

ASSERT_EQ(RCL_RET_OK, rcl_wait_set_add_timer(&wait_set, &timer, NULL)) <<
Expand Down