From aa5bd8794038ac3a8f7c44d8e816445017e86cd2 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Tue, 14 Apr 2020 03:48:13 -0300 Subject: [PATCH 01/15] Add tests for the test_validate_enclave_name module Signed-off-by: Jorge Perez --- rcl/test/CMakeLists.txt | 6 ++ rcl/test/rcl/test_validate_enclave_name.cpp | 85 +++++++++++++++++++++ 2 files changed, 91 insertions(+) create mode 100644 rcl/test/rcl/test_validate_enclave_name.cpp diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 7a8fdc531..261f8bd82 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -316,6 +316,12 @@ target_link_libraries(test_rmw_impl_id_check_exe ${PROJECT_NAME}) call_for_each_rmw_implementation(test_target) +rcl_add_custom_gtest(test_validate_enclave_name + SRCS rcl/test_validate_enclave_name.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} +) + rcl_add_custom_gtest(test_validate_topic_name SRCS rcl/test_validate_topic_name.cpp APPEND_LIBRARY_DIRS ${extra_lib_dirs} diff --git a/rcl/test/rcl/test_validate_enclave_name.cpp b/rcl/test/rcl/test_validate_enclave_name.cpp new file mode 100644 index 000000000..689d216ab --- /dev/null +++ b/rcl/test/rcl/test_validate_enclave_name.cpp @@ -0,0 +1,85 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/rcl.h" +#include "rcl/validate_enclave_name.h" + +#include "rcl/error_handling.h" + +TEST(TestValidateEnclaveName, test_validate) { + int validation_result; + size_t invalid_index; + + EXPECT_EQ( + RMW_RET_INVALID_ARGUMENT, + rcl_validate_enclave_name(nullptr, &validation_result, &invalid_index)); + + EXPECT_EQ( + RMW_RET_INVALID_ARGUMENT, + rcl_validate_enclave_name_with_size(nullptr, 20, &validation_result, &invalid_index)); + + EXPECT_EQ( + RMW_RET_INVALID_ARGUMENT, + rcl_validate_enclave_name_with_size("/foo", 20, nullptr, &invalid_index)); + + EXPECT_EQ( + RCL_RET_OK, + rcl_validate_enclave_name("/foo", &validation_result, &invalid_index)); + EXPECT_EQ(RCL_ENCLAVE_NAME_VALID, validation_result); + + EXPECT_EQ( + RCL_RET_OK, + rcl_validate_enclave_name("/foo/bar", &validation_result, &invalid_index)); + EXPECT_EQ(RCL_ENCLAVE_NAME_VALID, validation_result); +} + +TEST(TestValidateEnclaveName, test_validation_string) { + struct enclave_case + { + std::string enclave; + int expected_validation_result; + size_t expected_invalid_index; + }; + std::vector enclave_cases_that_should_fail = { + // TODO(blast_545): Look for naming conventions doc for enclave_names + {"", RCL_ENCLAVE_NAME_INVALID_IS_EMPTY_STRING, 0}, + {"~/foo", RCL_ENCLAVE_NAME_INVALID_NOT_ABSOLUTE, 0}, + {"/foo/", RCL_ENCLAVE_NAME_INVALID_ENDS_WITH_FORWARD_SLASH, 4}, + {"/foo/$", RCL_ENCLAVE_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS, 5}, + {"/bar#", RCL_ENCLAVE_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS, 4}, + {"/foo//bar", RCL_ENCLAVE_NAME_INVALID_CONTAINS_REPEATED_FORWARD_SLASH, 5}, + {"/1bar", RCL_ENCLAVE_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER, 1} + }; + for (const auto & case_tuple : enclave_cases_that_should_fail) { + std::string enclave = case_tuple.enclave; + int expected_validation_result = case_tuple.expected_validation_result; + size_t expected_invalid_index = case_tuple.expected_invalid_index; + int validation_result; + size_t invalid_index = 0; + rcl_ret_t ret = rcl_validate_enclave_name(enclave.c_str(), &validation_result, &invalid_index); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(expected_validation_result, validation_result) << + "'" << enclave << "' should have failed with '" << + expected_validation_result << "' but got '" << validation_result << "'.\n" << + " " << std::string(invalid_index, ' ') << "^"; + EXPECT_EQ(expected_invalid_index, invalid_index) << + "Enclave '" << enclave << "' failed with '" << validation_result << "'."; + EXPECT_NE(nullptr, rcl_enclave_name_validation_result_string(validation_result)) << enclave; + } +} From 55c7e0eb43750dfc9b5920c64bde48aedac6ef99 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Thu, 16 Apr 2020 17:06:52 -0300 Subject: [PATCH 02/15] Add test that is failing Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 76 ++++++++++++++++++++++++++++-- 1 file changed, 73 insertions(+), 3 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index f13efb6d6..dff3219b5 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -290,7 +290,7 @@ TEST_F( const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); const char * topic = "rcl_test_subscription_nominal_string_sequence_chatter"; - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( @@ -300,7 +300,7 @@ TEST_F( }); rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); - ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { @@ -320,7 +320,7 @@ TEST_F( ret = rcl_publish(&publisher, &msg, nullptr); ret = rcl_publish(&publisher, &msg, nullptr); test_msgs__msg__Strings__fini(&msg); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } bool success; wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); @@ -430,3 +430,73 @@ TEST_F( std::string(seq->data[0].string_value.data, seq->data[0].string_value.size)); } } + +/* Basic nominal test of a subscription with take_serialize msg + */ +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_serialized) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + rcutils_allocator_t allocator = rcl_get_default_allocator(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + const char * topic = "/chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); + auto initial_capacity_ser = 0u; + ASSERT_EQ( + RCL_RET_OK, rmw_serialized_message_init( + &serialized_msg, initial_capacity_ser, &allocator)) << rcl_get_error_string().str; + const char * test_string = "testing"; + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + + // TEST BEFORE SERIALIZE, works + ASSERT_STREQ(msg.string_value.data, test_string); + + ret = rmw_serialize(&msg, ts, &serialized_msg); + ASSERT_EQ(RMW_RET_OK, ret); + + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_reset_error(); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + { + ret = rcl_publish(&publisher, &serialized_msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + bool success; + wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); + ASSERT_TRUE(success); + { + rcl_serialized_message_t serialized_msg_rcv = rmw_get_zero_initialized_serialized_message(); + initial_capacity_ser = 0u; + ASSERT_EQ( + RCL_RET_OK, rmw_serialized_message_init( + &serialized_msg_rcv, initial_capacity_ser, &allocator)) << rcl_get_error_string().str; + ret = rcl_take_serialized_message(&subscription, &serialized_msg_rcv, nullptr, nullptr); + ASSERT_EQ(RMW_RET_OK, ret); + + test_msgs__msg__Strings msg_rcv; + test_msgs__msg__Strings__init(&msg_rcv); + ret = rmw_deserialize(&serialized_msg_rcv, ts, &msg_rcv); + ASSERT_EQ(RMW_RET_OK, ret); + ASSERT_STREQ(test_string, msg_rcv.string_value.data); + } +} From b47c41af4a9297291aafbd44efd237c771895a55 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Thu, 16 Apr 2020 18:04:27 -0300 Subject: [PATCH 03/15] Fix function used for publishing Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index dff3219b5..3246554cf 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -478,7 +478,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription std::this_thread::sleep_for(std::chrono::milliseconds(1000)); { - ret = rcl_publish(&publisher, &serialized_msg, nullptr); + ret = rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } bool success; From 550471c4f9617adb8f236044eb89907fe5f43259 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Mon, 20 Apr 2020 01:58:13 -0300 Subject: [PATCH 04/15] Fix serialize subscription test Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 3246554cf..a03a005b5 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -438,8 +438,8 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); rcutils_allocator_t allocator = rcl_get_default_allocator(); const rosidl_message_type_support_t * ts = - ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); - const char * topic = "/chatter"; + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + const char * topic = "/chatterSer"; rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -458,10 +458,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription test_msgs__msg__Strings msg; test_msgs__msg__Strings__init(&msg); ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); - - // TEST BEFORE SERIALIZE, works ASSERT_STREQ(msg.string_value.data, test_string); - ret = rmw_serialize(&msg, ts, &serialized_msg); ASSERT_EQ(RMW_RET_OK, ret); @@ -497,6 +494,8 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription test_msgs__msg__Strings__init(&msg_rcv); ret = rmw_deserialize(&serialized_msg_rcv, ts, &msg_rcv); ASSERT_EQ(RMW_RET_OK, ret); - ASSERT_STREQ(test_string, msg_rcv.string_value.data); + // ASSERT_STREQ(test_string, msg_rcv.string_value.data); + ASSERT_EQ( + std::string(test_string), std::string(msg_rcv.string_value.data, msg_rcv.string_value.size)); } } From 45bd4499f2ecbe418e95dbdfa10c10068a9f6eb0 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Mon, 20 Apr 2020 04:45:56 -0300 Subject: [PATCH 05/15] Add tests for loan msgs and get_options Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 91 +++++++++++++++++++++++++++++- 1 file changed, 90 insertions(+), 1 deletion(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index a03a005b5..de14fde7b 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -494,8 +494,97 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription test_msgs__msg__Strings__init(&msg_rcv); ret = rmw_deserialize(&serialized_msg_rcv, ts, &msg_rcv); ASSERT_EQ(RMW_RET_OK, ret); - // ASSERT_STREQ(test_string, msg_rcv.string_value.data); ASSERT_EQ( std::string(test_string), std::string(msg_rcv.string_value.data, msg_rcv.string_value.size)); } } + +/* Basic test for subscription loan functions + */ +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_loaned) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + const char * topic = "rcl_loan"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + // 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(1000)); + const char * test_string = "testing"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + bool success; + wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); + ASSERT_TRUE(success); + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings * msg_loaned; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + + // Only if rmw supports the functionality + if (rcl_subscription_can_loan_messages(&subscription)) { + ret = rcl_take_loaned_message( + &subscription, reinterpret_cast(&msg_loaned), nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ( + std::string(test_string), + std::string(*msg_loaned->string_value.data, msg_loaned->string_value.size)); + } else { + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ( + std::string(test_string), std::string(msg.string_value.data, msg.string_value.size)); + } + } +} + +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_get_options) { + rcl_ret_t ret; + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + const char * topic = "test_get_options"; + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + const rcl_subscription_options_t * get_sub_options = rcl_subscription_get_options(&subscription); + ASSERT_EQ(subscription_options.qos.history, get_sub_options->qos.history); + ASSERT_EQ(subscription_options.qos.depth, get_sub_options->qos.depth); + ASSERT_EQ(subscription_options.qos.durability, get_sub_options->qos.durability); + + ASSERT_EQ(NULL, rcl_subscription_get_options(nullptr)); +} From 959d02e657ae198672881c7f4311f0331a71718b Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Thu, 23 Apr 2020 19:13:59 -0500 Subject: [PATCH 06/15] Add support for taking a sequence of messages (#614) * Add support for taking a sequence of messages Signed-off-by: Michael Carroll * Fix uncrustify Signed-off-by: Michael Carroll * Apply suggestions from code review Co-Authored-By: Jacob Perron Signed-off-by: Michael Carroll * Address reviewer feedback. Signed-off-by: Michael Carroll * Remove vertical whitespace Signed-off-by: Michael Carroll * Address reviewer feedback Signed-off-by: Michael Carroll * Remove blank line Signed-off-by: Michael Carroll Co-authored-by: Jacob Perron Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 156 ++++++++++++++++++++++++++++- 1 file changed, 155 insertions(+), 1 deletion(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index de14fde7b..373f3ea0f 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -507,7 +507,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); const char * topic = "rcl_loan"; - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( @@ -566,6 +566,160 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription } } + + +/* Basic nominal test of a subscription taking a sequence. + */ +TEST_F( + CLASSNAME( + TestSubscriptionFixture, + RMW_IMPLEMENTATION), test_subscription_nominal_string_sequence) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + const char * topic = "rcl_test_subscription_nominal_string_sequence_chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + // 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(1000)); + const char * test_string = "testing"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + ret = rcl_publish(&publisher, &msg, nullptr); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + bool success; + wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); + ASSERT_TRUE(success); + auto allocator = rcutils_get_default_allocator(); + { + size_t size = 1; + rmw_message_info_sequence_t message_infos; + rmw_message_info_sequence_init(&message_infos, size, &allocator); + + rmw_message_sequence_t messages; + rmw_message_sequence_init(&messages, size, &allocator); + + auto seq = test_msgs__msg__Strings__Sequence__create(size); + + for (size_t ii = 0; ii < size; ++ii) { + messages.data[ii] = &seq->data[ii]; + } + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rmw_message_info_sequence_fini(&message_infos); + rmw_message_sequence_fini(&messages); + test_msgs__msg__Strings__Sequence__destroy(seq); + }); + + // Attempt to take more than capacity allows. + ret = rcl_take_sequence(&subscription, 5, &messages, &message_infos, nullptr); + ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + + ASSERT_EQ(0u, messages.size); + ASSERT_EQ(0u, message_infos.size); + } + + { + size_t size = 5; + rmw_message_info_sequence_t message_infos; + rmw_message_info_sequence_init(&message_infos, size, &allocator); + + rmw_message_sequence_t messages; + rmw_message_sequence_init(&messages, size, &allocator); + + auto seq = test_msgs__msg__Strings__Sequence__create(size); + + for (size_t ii = 0; ii < size; ++ii) { + messages.data[ii] = &seq->data[ii]; + } + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rmw_message_info_sequence_fini(&message_infos); + rmw_message_sequence_fini(&messages); + test_msgs__msg__Strings__Sequence__destroy(seq); + }); + + ret = rcl_take_sequence(&subscription, 5, &messages, &message_infos, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(3u, messages.size); + ASSERT_EQ(3u, message_infos.size); + } + + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + ret = rcl_publish(&publisher, &msg, nullptr); + ret = rcl_publish(&publisher, &msg, nullptr); + ret = rcl_publish(&publisher, &msg, nullptr); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + // Give a brief moment for publications to go through. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + // Take fewer messages than are available in the subscription + { + size_t size = 3; + rmw_message_info_sequence_t message_infos; + rmw_message_info_sequence_init(&message_infos, size, &allocator); + + rmw_message_sequence_t messages; + rmw_message_sequence_init(&messages, size, &allocator); + + auto seq = test_msgs__msg__Strings__Sequence__create(size); + + for (size_t ii = 0; ii < size; ++ii) { + messages.data[ii] = &seq->data[ii]; + } + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rmw_message_info_sequence_fini(&message_infos); + rmw_message_sequence_fini(&messages); + test_msgs__msg__Strings__Sequence__destroy(seq); + }); + + ret = rcl_take_sequence(&subscription, 3, &messages, &message_infos, nullptr); + + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(3u, messages.size); + ASSERT_EQ(3u, message_infos.size); + + ASSERT_EQ( + std::string(test_string), + std::string(seq->data[0].string_value.data, seq->data[0].string_value.size)); + } +} + TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_get_options) { rcl_ret_t ret; const rosidl_message_type_support_t * ts = From 41bef6d741adc43238e5ca0dc919ca238f9c2a88 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Thu, 16 Apr 2020 17:06:52 -0300 Subject: [PATCH 07/15] Add test that is failing Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 79 ++++++++++++++++++++++++++++++ 1 file changed, 79 insertions(+) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 373f3ea0f..831b695b1 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -742,3 +742,82 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_get_options) ASSERT_EQ(NULL, rcl_subscription_get_options(nullptr)); } + +/* Basic nominal test of a subscription with take_serialize msg + */ +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_serialized) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + rcutils_allocator_t allocator = rcl_get_default_allocator(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + const char * topic = "/chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); + auto initial_capacity_ser = 0u; + ASSERT_EQ( + RCL_RET_OK, rmw_serialized_message_init( + &serialized_msg, initial_capacity_ser, &allocator)) << rcl_get_error_string().str; + const char * test_string = "testing"; + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + + // TEST BEFORE SERIALIZE, works + ASSERT_STREQ(msg.string_value.data, test_string); + + ret = rmw_serialize(&msg, ts, &serialized_msg); + ASSERT_EQ(RMW_RET_OK, ret); + + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_reset_error(); + + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + { + ret = rcl_publish(&publisher, &serialized_msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + bool success; + wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); + ASSERT_TRUE(success); + { + rcl_serialized_message_t serialized_msg_rcv = rmw_get_zero_initialized_serialized_message(); + initial_capacity_ser = 0u; + ASSERT_EQ( + RCL_RET_OK, rmw_serialized_message_init( + &serialized_msg_rcv, initial_capacity_ser, &allocator)) << rcl_get_error_string().str; + ret = rcl_take_serialized_message(&subscription, &serialized_msg_rcv, nullptr, nullptr); + ASSERT_EQ(RMW_RET_OK, ret); + + // Compare serialized version + // This test fails too, suggesting a problem with the resializing mechanism stage + for (size_t i = 0; i < serialized_msg_rcv.buffer_length; ++i) { + EXPECT_EQ(serialized_msg.buffer[i], serialized_msg_rcv.buffer[i]); + } + + // This is the test that I'd like to get working + // The rcv decoded string does not match the original one + // And, in fact, is not the same for each rmw vendor + test_msgs__msg__Strings msg_rcv; + test_msgs__msg__Strings__init(&msg_rcv); + ret = rmw_deserialize(&serialized_msg_rcv, ts, &msg_rcv); + ASSERT_EQ(RMW_RET_OK, ret); + ASSERT_STREQ(test_string, msg_rcv.string_value.data); + } +} From 00286ddbf2f26ecfb07b0e23e93ded3738c444fb Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Thu, 16 Apr 2020 18:04:27 -0300 Subject: [PATCH 08/15] Fix function used for publishing Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 831b695b1..52bd1152b 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -790,7 +790,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription std::this_thread::sleep_for(std::chrono::milliseconds(1000)); { - ret = rcl_publish(&publisher, &serialized_msg, nullptr); + ret = rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } bool success; @@ -806,7 +806,6 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription ASSERT_EQ(RMW_RET_OK, ret); // Compare serialized version - // This test fails too, suggesting a problem with the resializing mechanism stage for (size_t i = 0; i < serialized_msg_rcv.buffer_length; ++i) { EXPECT_EQ(serialized_msg.buffer[i], serialized_msg_rcv.buffer[i]); } From 07a433713f073a27be2c2efaf55ee4162bed62ab Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Mon, 20 Apr 2020 01:58:13 -0300 Subject: [PATCH 09/15] Fix serialize subscription test Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 52bd1152b..8c91973de 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -750,9 +750,9 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); rcutils_allocator_t allocator = rcl_get_default_allocator(); const rosidl_message_type_support_t * ts = - ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); - const char * topic = "/chatter"; - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + const char * topic = "/chatterSer"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( @@ -770,10 +770,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription test_msgs__msg__Strings msg; test_msgs__msg__Strings__init(&msg); ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); - - // TEST BEFORE SERIALIZE, works ASSERT_STREQ(msg.string_value.data, test_string); - ret = rmw_serialize(&msg, ts, &serialized_msg); ASSERT_EQ(RMW_RET_OK, ret); @@ -817,6 +814,8 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription test_msgs__msg__Strings__init(&msg_rcv); ret = rmw_deserialize(&serialized_msg_rcv, ts, &msg_rcv); ASSERT_EQ(RMW_RET_OK, ret); - ASSERT_STREQ(test_string, msg_rcv.string_value.data); + // ASSERT_STREQ(test_string, msg_rcv.string_value.data); + ASSERT_EQ( + std::string(test_string), std::string(msg_rcv.string_value.data, msg_rcv.string_value.size)); } } From 73d7c8a375da5cf9dcd0f0ce7ac28e48c2b2b06e Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Mon, 20 Apr 2020 04:45:56 -0300 Subject: [PATCH 10/15] Add tests for loan msgs and get_options Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 8c91973de..3825be461 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -814,7 +814,6 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription test_msgs__msg__Strings__init(&msg_rcv); ret = rmw_deserialize(&serialized_msg_rcv, ts, &msg_rcv); ASSERT_EQ(RMW_RET_OK, ret); - // ASSERT_STREQ(test_string, msg_rcv.string_value.data); ASSERT_EQ( std::string(test_string), std::string(msg_rcv.string_value.data, msg_rcv.string_value.size)); } From 1c3afe1cf0819f5856cb9684afd5086000991129 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Fri, 24 Apr 2020 14:25:40 -0300 Subject: [PATCH 11/15] Remove extra character left when rebasing Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 23 ----------------------- 1 file changed, 23 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 3825be461..c0ff48f79 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -720,29 +720,6 @@ TEST_F( } } -TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_get_options) { - rcl_ret_t ret; - const rosidl_message_type_support_t * ts = - ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); - const char * topic = "test_get_options"; - rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); - rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); - ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - }); - - const rcl_subscription_options_t * get_sub_options = rcl_subscription_get_options(&subscription); - ASSERT_EQ(subscription_options.qos.history, get_sub_options->qos.history); - ASSERT_EQ(subscription_options.qos.depth, get_sub_options->qos.depth); - ASSERT_EQ(subscription_options.qos.durability, get_sub_options->qos.durability); - - ASSERT_EQ(NULL, rcl_subscription_get_options(nullptr)); -} - /* Basic nominal test of a subscription with take_serialize msg */ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_serialized) { From e0bfb9e8c400451530ed993af2922c498e2d2710 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Fri, 24 Apr 2020 16:30:44 -0300 Subject: [PATCH 12/15] Remove test used for debugging Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index c0ff48f79..bb7c6db74 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -779,14 +779,6 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription ret = rcl_take_serialized_message(&subscription, &serialized_msg_rcv, nullptr, nullptr); ASSERT_EQ(RMW_RET_OK, ret); - // Compare serialized version - for (size_t i = 0; i < serialized_msg_rcv.buffer_length; ++i) { - EXPECT_EQ(serialized_msg.buffer[i], serialized_msg_rcv.buffer[i]); - } - - // This is the test that I'd like to get working - // The rcv decoded string does not match the original one - // And, in fact, is not the same for each rmw vendor test_msgs__msg__Strings msg_rcv; test_msgs__msg__Strings__init(&msg_rcv); ret = rmw_deserialize(&serialized_msg_rcv, ts, &msg_rcv); From e3be2cae3aad4fa2cf52c4b414dd51bededa3bf9 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Fri, 24 Apr 2020 17:25:58 -0300 Subject: [PATCH 13/15] Remove repeated tests Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 222 ----------------------------- 1 file changed, 222 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index bb7c6db74..1f4cc0adf 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -565,225 +565,3 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription } } } - - - -/* Basic nominal test of a subscription taking a sequence. - */ -TEST_F( - CLASSNAME( - TestSubscriptionFixture, - RMW_IMPLEMENTATION), test_subscription_nominal_string_sequence) { - rcl_ret_t ret; - rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); - const rosidl_message_type_support_t * ts = - ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); - const char * topic = "rcl_test_subscription_nominal_string_sequence_chatter"; - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); - ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - }); - rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); - rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); - ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - }); - // 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(1000)); - const char * test_string = "testing"; - { - test_msgs__msg__Strings msg; - test_msgs__msg__Strings__init(&msg); - ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); - ret = rcl_publish(&publisher, &msg, nullptr); - ret = rcl_publish(&publisher, &msg, nullptr); - ret = rcl_publish(&publisher, &msg, nullptr); - test_msgs__msg__Strings__fini(&msg); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - } - bool success; - wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); - ASSERT_TRUE(success); - auto allocator = rcutils_get_default_allocator(); - { - size_t size = 1; - rmw_message_info_sequence_t message_infos; - rmw_message_info_sequence_init(&message_infos, size, &allocator); - - rmw_message_sequence_t messages; - rmw_message_sequence_init(&messages, size, &allocator); - - auto seq = test_msgs__msg__Strings__Sequence__create(size); - - for (size_t ii = 0; ii < size; ++ii) { - messages.data[ii] = &seq->data[ii]; - } - - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - rmw_message_info_sequence_fini(&message_infos); - rmw_message_sequence_fini(&messages); - test_msgs__msg__Strings__Sequence__destroy(seq); - }); - - // Attempt to take more than capacity allows. - ret = rcl_take_sequence(&subscription, 5, &messages, &message_infos, nullptr); - ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; - - ASSERT_EQ(0u, messages.size); - ASSERT_EQ(0u, message_infos.size); - } - - { - size_t size = 5; - rmw_message_info_sequence_t message_infos; - rmw_message_info_sequence_init(&message_infos, size, &allocator); - - rmw_message_sequence_t messages; - rmw_message_sequence_init(&messages, size, &allocator); - - auto seq = test_msgs__msg__Strings__Sequence__create(size); - - for (size_t ii = 0; ii < size; ++ii) { - messages.data[ii] = &seq->data[ii]; - } - - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - rmw_message_info_sequence_fini(&message_infos); - rmw_message_sequence_fini(&messages); - test_msgs__msg__Strings__Sequence__destroy(seq); - }); - - ret = rcl_take_sequence(&subscription, 5, &messages, &message_infos, nullptr); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_EQ(3u, messages.size); - ASSERT_EQ(3u, message_infos.size); - } - - { - test_msgs__msg__Strings msg; - test_msgs__msg__Strings__init(&msg); - ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); - ret = rcl_publish(&publisher, &msg, nullptr); - ret = rcl_publish(&publisher, &msg, nullptr); - ret = rcl_publish(&publisher, &msg, nullptr); - ret = rcl_publish(&publisher, &msg, nullptr); - ret = rcl_publish(&publisher, &msg, nullptr); - test_msgs__msg__Strings__fini(&msg); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - } - - // Give a brief moment for publications to go through. - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - // Take fewer messages than are available in the subscription - { - size_t size = 3; - rmw_message_info_sequence_t message_infos; - rmw_message_info_sequence_init(&message_infos, size, &allocator); - - rmw_message_sequence_t messages; - rmw_message_sequence_init(&messages, size, &allocator); - - auto seq = test_msgs__msg__Strings__Sequence__create(size); - - for (size_t ii = 0; ii < size; ++ii) { - messages.data[ii] = &seq->data[ii]; - } - - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - rmw_message_info_sequence_fini(&message_infos); - rmw_message_sequence_fini(&messages); - test_msgs__msg__Strings__Sequence__destroy(seq); - }); - - ret = rcl_take_sequence(&subscription, 3, &messages, &message_infos, nullptr); - - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_EQ(3u, messages.size); - ASSERT_EQ(3u, message_infos.size); - - ASSERT_EQ( - std::string(test_string), - std::string(seq->data[0].string_value.data, seq->data[0].string_value.size)); - } -} - -/* Basic nominal test of a subscription with take_serialize msg - */ -TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_serialized) { - rcl_ret_t ret; - rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); - rcutils_allocator_t allocator = rcl_get_default_allocator(); - const rosidl_message_type_support_t * ts = - ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); - const char * topic = "/chatterSer"; - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); - ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - }); - - rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); - auto initial_capacity_ser = 0u; - ASSERT_EQ( - RCL_RET_OK, rmw_serialized_message_init( - &serialized_msg, initial_capacity_ser, &allocator)) << rcl_get_error_string().str; - const char * test_string = "testing"; - test_msgs__msg__Strings msg; - test_msgs__msg__Strings__init(&msg); - ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); - ASSERT_STREQ(msg.string_value.data, test_string); - ret = rmw_serialize(&msg, ts, &serialized_msg); - ASSERT_EQ(RMW_RET_OK, ret); - - rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); - rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); - ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - ret = rcl_subscription_fini(&subscription, this->node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - }); - rcl_reset_error(); - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - { - ret = rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - } - bool success; - wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); - ASSERT_TRUE(success); - { - rcl_serialized_message_t serialized_msg_rcv = rmw_get_zero_initialized_serialized_message(); - initial_capacity_ser = 0u; - ASSERT_EQ( - RCL_RET_OK, rmw_serialized_message_init( - &serialized_msg_rcv, initial_capacity_ser, &allocator)) << rcl_get_error_string().str; - ret = rcl_take_serialized_message(&subscription, &serialized_msg_rcv, nullptr, nullptr); - ASSERT_EQ(RMW_RET_OK, ret); - - test_msgs__msg__Strings msg_rcv; - test_msgs__msg__Strings__init(&msg_rcv); - ret = rmw_deserialize(&serialized_msg_rcv, ts, &msg_rcv); - ASSERT_EQ(RMW_RET_OK, ret); - ASSERT_EQ( - std::string(test_string), std::string(msg_rcv.string_value.data, msg_rcv.string_value.size)); - } -} From 1861c61c90bfe5597facf4336fac22c53adfccbf Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Fri, 24 Apr 2020 17:27:54 -0300 Subject: [PATCH 14/15] Add test for get_options Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 1f4cc0adf..fa0837805 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -565,3 +565,26 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription } } } + +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_get_options) { + rcl_ret_t ret; + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + const char * topic = "test_get_options"; + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + const rcl_subscription_options_t * get_sub_options = rcl_subscription_get_options(&subscription); + ASSERT_EQ(subscription_options.qos.history, get_sub_options->qos.history); + ASSERT_EQ(subscription_options.qos.depth, get_sub_options->qos.depth); + ASSERT_EQ(subscription_options.qos.durability, get_sub_options->qos.durability); + + ASSERT_EQ(NULL, rcl_subscription_get_options(nullptr)); +} From a967df41fc3da690264f8b97535d35c0353a3302 Mon Sep 17 00:00:00 2001 From: Jorge Perez Date: Fri, 24 Apr 2020 17:36:08 -0300 Subject: [PATCH 15/15] Fix uncrustify errors Signed-off-by: Jorge Perez --- rcl/test/rcl/test_subscription.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index fa0837805..2b92e7047 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -290,7 +290,7 @@ TEST_F( const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); const char * topic = "rcl_test_subscription_nominal_string_sequence_chatter"; - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( @@ -300,7 +300,7 @@ TEST_F( }); rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); - ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( { @@ -320,7 +320,7 @@ TEST_F( ret = rcl_publish(&publisher, &msg, nullptr); ret = rcl_publish(&publisher, &msg, nullptr); test_msgs__msg__Strings__fini(&msg); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } bool success; wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); @@ -507,7 +507,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); const char * topic = "rcl_loan"; - rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(