From bf4296c0b260987847cd4f314ec08a75fba052d9 Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Mon, 25 Mar 2019 14:01:14 -0300 Subject: [PATCH 1/8] Add rcl_publisher_get_actual_qos function Signed-off-by: ivanpauno --- rcl/include/rcl/publisher.h | 28 +++++++++++++++++++++++++++- rcl/src/rcl/publisher.c | 23 +++++++++++++++++++++++ 2 files changed, 50 insertions(+), 1 deletion(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index beb130118..0f5ffd054 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -455,7 +455,7 @@ rcl_publisher_is_valid_except_context(const rcl_publisher_t * publisher); * \param[out] subscription_count number of matched subscriptions * \return `RCL_RET_OK` if the count was retrieved, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or - * \return `RCL_RET_SUBSCRIPTION_INVALID` if the subscription is invalid, or + * \return `RCL_RET_PUBLISHER_INVALID` if the publisher is invalid, or * \return `RCL_RET_ERROR` if an unspecified error occurs. */ RCL_PUBLIC @@ -465,6 +465,32 @@ rcl_publisher_get_subscription_count( const rcl_publisher_t * publisher, size_t * subscription_count); +/// Get the actual qos settings of the publisher. +/** + * Used to get the actual qos settings of the publisher. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] publisher pointer to the rcl publisher + * \param[out] qos the actual qos settings + * \return `RCL_RET_OK` if the count was retrieved, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RCL_RET_PUBLISHER_INVALID` if the publisher is invalid, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_ret_t +rcl_publisher_get_actual_qos( + const rcl_publisher_t * publisher, + rmw_qos_profile_t * qos); + #ifdef __cplusplus } #endif diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 8709a9905..84f5adeb8 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -349,6 +349,29 @@ rcl_publisher_get_subscription_count( return RCL_RET_OK; } +rmw_ret_t +rcl_publisher_get_actual_qos( + const rcl_publisher_t * publisher, + rmw_qos_profile_t * qos) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(qos, RCL_RET_INVALID_ARGUMENT); + + rmw_ret_t ret = rmw_publisher_get_actual_qos(publisher->impl->rmw_handle, + qos); + + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + qos->avoid_ros_namespace_conventions = + publisher->impl->options.qos.avoid_ros_namespace_conventions; + + return RCL_RET_OK; +} + #ifdef __cplusplus } #endif From 0f34fb77dc311841cdbbd76cc26d0435ecc40d71 Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Mon, 25 Mar 2019 18:17:10 -0300 Subject: [PATCH 2/8] Add test for rcl_publisher_get_actual_qos Signed-off-by: ivanpauno --- rcl/test/CMakeLists.txt | 8 ++ rcl/test/rcl/test_get_actual_qos.cpp | 181 +++++++++++++++++++++++++++ 2 files changed, 189 insertions(+) create mode 100644 rcl/test/rcl/test_get_actual_qos.cpp diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index cd4e77fc5..c206d3dc2 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -119,6 +119,14 @@ function(test_target_function) AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) + rcl_add_custom_gtest(test_get_actual_qos${target_suffix} + SRCS rcl/test_get_actual_qos.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + rcl_add_custom_gtest(test_init${target_suffix} SRCS rcl/test_init.cpp ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var} diff --git a/rcl/test/rcl/test_get_actual_qos.cpp b/rcl/test/rcl/test_get_actual_qos.cpp new file mode 100644 index 000000000..fdccf6cdc --- /dev/null +++ b/rcl/test/rcl/test_get_actual_qos.cpp @@ -0,0 +1,181 @@ +// Copyright 2018 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 + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl/publisher.h" + +#include "rcutils/logging_macros.h" + +#include "test_msgs/msg/primitives.h" +#include "test_msgs/srv/primitives.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +#define TEST_FIXTURE_P_RMW(test_fixture_name) CLASSNAME(test_fixture_name, \ + RMW_IMPLEMENTATION) +#define APPLY(macro, ...) macro(__VA_ARGS__) +#define TEST_P_RMW(test_case_name, test_name) APPLY(TEST_P, \ + CLASSNAME(test_case_name, RMW_IMPLEMENTATION), test_name) +#define INSTANTIATE_TEST_CASE_P_RMW(instance_name, test_case_name, ...) APPLY( \ + INSTANTIATE_TEST_CASE_P, instance_name, CLASSNAME(test_case_name, \ + RMW_IMPLEMENTATION), __VA_ARGS__) + +/** + * Parameterized test. + * The first param are the NodeOptions used to create the nodes. + * The second param are the expect intraprocess count results. + */ +struct CLASSNAME (TestParameters, RMW_IMPLEMENTATION) +{ +rmw_qos_profile_t qos_to_set; +rmw_qos_profile_t qos_expected; +std::string description; +}; + +std::ostream & operator<<( + std::ostream & out, + const CLASSNAME(TestParameters, RMW_IMPLEMENTATION) & params) +{ + out << params.description; + return out; +} + + +class CLASSNAME (TestGetActualQoS, RMW_IMPLEMENTATION) + : public ::testing::TestWithParam +{ +public: + rcl_node_t * node_ptr; + rcl_context_t * context_ptr; + void SetUp() + { + rcl_ret_t ret; + rcl_node_options_t node_options = rcl_node_get_default_options(); + + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "test_get_actual_qos_node"; + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret; + + ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +TEST_P(CLASSNAME(TestGetActualQoS, RMW_IMPLEMENTATION), test_publisher_get_qos_settings) { + CLASSNAME(TestParameters, RMW_IMPLEMENTATION) parameters = + GetParam(); + std::string topic_name("/test_publisher_get_actual_qos__"); + rcl_ret_t ret; + + rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); + pub_ops.qos = parameters.qos_to_set; + auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Primitives); + ret = rcl_publisher_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + rmw_qos_profile_t qos; + ret = rcl_publisher_get_actual_qos(&pub, &qos); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ( + parameters.qos_to_set.history, + parameters.qos_expected.history); + EXPECT_EQ( + parameters.qos_to_set.depth, + parameters.qos_expected.depth); + EXPECT_EQ( + parameters.qos_to_set.reliability, + parameters.qos_expected.reliability); + EXPECT_EQ( + parameters.qos_to_set.durability, + parameters.qos_expected.durability); + EXPECT_EQ( + parameters.qos_to_set.avoid_ros_namespace_conventions, + parameters.qos_expected.avoid_ros_namespace_conventions); + + ret = rcl_publisher_fini(&pub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +CLASSNAME(TestParameters, RMW_IMPLEMENTATION) parameters[] = { + /* + Testing with default qos settings. + */ + { + rmw_qos_profile_default, + rmw_qos_profile_default, + "publisher_default_qos" + }, + /* + Test with non-default settings. + */ + { + { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1000, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + true + }, + { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1000, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + true + }, + "publisher_non_default_qos" + } +}; + +INSTANTIATE_TEST_CASE_P_RMW( + TestWithDifferentQoSSettings, + TestGetActualQoS, + ::testing::ValuesIn(parameters), + ::testing::PrintToStringParamName()); From 955139566e7ddacdf66329e05f445e9f8268069a Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Mon, 25 Mar 2019 18:20:27 -0300 Subject: [PATCH 3/8] Modified doc string Signed-off-by: ivanpauno --- rcl/include/rcl/publisher.h | 1 + 1 file changed, 1 insertion(+) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 0f5ffd054..e2f1c02c5 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -468,6 +468,7 @@ rcl_publisher_get_subscription_count( /// Get the actual qos settings of the publisher. /** * Used to get the actual qos settings of the publisher. + * This resolves what is the actual value of RMW_*_SYSTEM_DEFAULT. * *
* Attribute | Adherence From cbd46cbe69365b5a60de4f86312aebd3d3cacf59 Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Tue, 26 Mar 2019 15:33:11 -0300 Subject: [PATCH 4/8] Corrected with PR comments Signed-off-by: ivanpauno --- rcl/include/rcl/publisher.h | 6 +- rcl/test/CMakeLists.txt | 2 +- rcl/test/rcl/test_get_actual_qos.cpp | 122 +++++++++++++++++---------- 3 files changed, 82 insertions(+), 48 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index e2f1c02c5..c5a62c980 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -468,7 +468,11 @@ rcl_publisher_get_subscription_count( /// Get the actual qos settings of the publisher. /** * Used to get the actual qos settings of the publisher. - * This resolves what is the actual value of RMW_*_SYSTEM_DEFAULT. + * The actual configuration applied when using RMW_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the publisher, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_*_UNKNOWN. * *
* Attribute | Adherence diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index c206d3dc2..56fab0921 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -124,7 +124,7 @@ function(test_target_function) ENV ${rmw_implementation_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} LIBRARIES ${PROJECT_NAME} - AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + AMENT_DEPENDENCIES ${rmw_implementation} "test_msgs" ) rcl_add_custom_gtest(test_init${target_suffix} diff --git a/rcl/test/rcl/test_get_actual_qos.cpp b/rcl/test/rcl/test_get_actual_qos.cpp index fdccf6cdc..a1e641f1c 100644 --- a/rcl/test/rcl/test_get_actual_qos.cpp +++ b/rcl/test/rcl/test_get_actual_qos.cpp @@ -1,4 +1,4 @@ -// Copyright 2018 Open Source Robotics Foundation, Inc. +// Copyright 2019 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. @@ -11,11 +11,11 @@ // 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 +#include #include "rcl/error_handling.h" #include "rcl/rcl.h" @@ -42,29 +42,32 @@ INSTANTIATE_TEST_CASE_P, instance_name, CLASSNAME(test_case_name, \ RMW_IMPLEMENTATION), __VA_ARGS__) +// One const for each of the RMW_IMPLEMENTATION values +const char * rmw_fastrtps_cpp = "rmw_fastrtps_cpp"; +const char * rmw_fastrtps_dynamic_cpp = "rmw_fastrtps_dynamic_cpp"; + /** * Parameterized test. * The first param are the NodeOptions used to create the nodes. * The second param are the expect intraprocess count results. */ -struct CLASSNAME (TestParameters, RMW_IMPLEMENTATION) +struct TestParameters { -rmw_qos_profile_t qos_to_set; -rmw_qos_profile_t qos_expected; -std::string description; + rmw_qos_profile_t qos_to_set; + rmw_qos_profile_t qos_expected; + std::string description; }; std::ostream & operator<<( std::ostream & out, - const CLASSNAME(TestParameters, RMW_IMPLEMENTATION) & params) + const TestParameters & params) { out << params.description; return out; } - -class CLASSNAME (TestGetActualQoS, RMW_IMPLEMENTATION) - : public ::testing::TestWithParam +class TEST_FIXTURE_P_RMW (TestGetActualQoS) + : public ::testing::TestWithParam { public: rcl_node_t * node_ptr; @@ -105,9 +108,8 @@ class CLASSNAME (TestGetActualQoS, RMW_IMPLEMENTATION) } }; -TEST_P(CLASSNAME(TestGetActualQoS, RMW_IMPLEMENTATION), test_publisher_get_qos_settings) { - CLASSNAME(TestParameters, RMW_IMPLEMENTATION) parameters = - GetParam(); +TEST_P_RMW(TestGetActualQoS, test_publisher_get_qos_settings) { + TestParameters parameters = GetParam(); std::string topic_name("/test_publisher_get_actual_qos__"); rcl_ret_t ret; @@ -123,19 +125,19 @@ TEST_P(CLASSNAME(TestGetActualQoS, RMW_IMPLEMENTATION), test_publisher_get_qos_s ret = rcl_publisher_get_actual_qos(&pub, &qos); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; EXPECT_EQ( - parameters.qos_to_set.history, + qos.history, parameters.qos_expected.history); EXPECT_EQ( - parameters.qos_to_set.depth, + qos.depth, parameters.qos_expected.depth); EXPECT_EQ( - parameters.qos_to_set.reliability, + qos.reliability, parameters.qos_expected.reliability); EXPECT_EQ( - parameters.qos_to_set.durability, + qos.durability, parameters.qos_expected.durability); EXPECT_EQ( - parameters.qos_to_set.avoid_ros_namespace_conventions, + qos.avoid_ros_namespace_conventions, parameters.qos_expected.avoid_ros_namespace_conventions); ret = rcl_publisher_fini(&pub, this->node_ptr); @@ -143,39 +145,67 @@ TEST_P(CLASSNAME(TestGetActualQoS, RMW_IMPLEMENTATION), test_publisher_get_qos_s rcl_reset_error(); } -CLASSNAME(TestParameters, RMW_IMPLEMENTATION) parameters[] = { - /* - Testing with default qos settings. - */ - { - rmw_qos_profile_default, - rmw_qos_profile_default, - "publisher_default_qos" - }, - /* - Test with non-default settings. - */ - { +std::vector +get_parameters() +{ + std::vector + parameters({ + /* + Testing with default qos settings. + */ { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1000, - RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, - RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, - true + rmw_qos_profile_default, + rmw_qos_profile_default, + "publisher_default_qos" }, + /* + Test with non-default settings. + */ { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1000, - RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, - RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, - true - }, - "publisher_non_default_qos" + { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1000, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + true + }, + { + RMW_QOS_POLICY_HISTORY_KEEP_ALL, + 1000, + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + true + }, + "publisher_non_default_qos" + } + }); + rmw_qos_profile_t expected_system_default_qos; + +#ifdef RMW_IMPLEMENTATION + // RMW_IMPLEMENTATION is one of the const char * defined above + if (std::string("rmw_fastrtps_cpp").compare(RMW_IMPLEMENTATION) || + std::string("rmw_fastrtps_dynamic_cpp").compare(RMW_IMPLEMENTATION)) + { + expected_system_default_qos.history = + RMW_QOS_POLICY_HISTORY_KEEP_LAST; + expected_system_default_qos.depth = 1; + expected_system_default_qos.reliability = + RMW_QOS_POLICY_RELIABILITY_RELIABLE; + expected_system_default_qos.durability = + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + expected_system_default_qos.avoid_ros_namespace_conventions = + false; } -}; + parameters.push_back({ + rmw_qos_profile_system_default, + expected_system_default_qos, + "publisher_system_default_qos"}); +#endif + return parameters; +} INSTANTIATE_TEST_CASE_P_RMW( TestWithDifferentQoSSettings, TestGetActualQoS, - ::testing::ValuesIn(parameters), + ::testing::ValuesIn(get_parameters()), ::testing::PrintToStringParamName()); From 6695eadc8d840f619d906026394a868deec8a44f Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Wed, 27 Mar 2019 10:44:34 -0300 Subject: [PATCH 5/8] Correct RMW_IMPLEMENTATION checking. Add opensplice system default qos test Signed-off-by: ivanpauno --- rcl/test/rcl/test_get_actual_qos.cpp | 50 ++++++++++++++++------------ 1 file changed, 28 insertions(+), 22 deletions(-) diff --git a/rcl/test/rcl/test_get_actual_qos.cpp b/rcl/test/rcl/test_get_actual_qos.cpp index a1e641f1c..589dc198a 100644 --- a/rcl/test/rcl/test_get_actual_qos.cpp +++ b/rcl/test/rcl/test_get_actual_qos.cpp @@ -26,9 +26,13 @@ #include "test_msgs/msg/primitives.h" #include "test_msgs/srv/primitives.h" +#define STRINGIFY_(x) #x +#define STRINGIFY(x) STRINGIFY_(x) + #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +# define RMW_IMPLEMENTATION_STR STRINGIFY(RMW_IMPLEMENTATION) #else # define CLASSNAME(NAME, SUFFIX) NAME #endif @@ -42,10 +46,6 @@ INSTANTIATE_TEST_CASE_P, instance_name, CLASSNAME(test_case_name, \ RMW_IMPLEMENTATION), __VA_ARGS__) -// One const for each of the RMW_IMPLEMENTATION values -const char * rmw_fastrtps_cpp = "rmw_fastrtps_cpp"; -const char * rmw_fastrtps_dynamic_cpp = "rmw_fastrtps_dynamic_cpp"; - /** * Parameterized test. * The first param are the NodeOptions used to create the nodes. @@ -179,27 +179,33 @@ get_parameters() "publisher_non_default_qos" } }); - rmw_qos_profile_t expected_system_default_qos; -#ifdef RMW_IMPLEMENTATION - // RMW_IMPLEMENTATION is one of the const char * defined above - if (std::string("rmw_fastrtps_cpp").compare(RMW_IMPLEMENTATION) || - std::string("rmw_fastrtps_dynamic_cpp").compare(RMW_IMPLEMENTATION)) +#ifdef RMW_IMPLEMENTATION_STR + if (!std::string("rmw_fastrtps_cpp").compare(RMW_IMPLEMENTATION_STR) || + !std::string("rmw_fastrtps_dynamic_cpp").compare(RMW_IMPLEMENTATION_STR)) { - expected_system_default_qos.history = - RMW_QOS_POLICY_HISTORY_KEEP_LAST; - expected_system_default_qos.depth = 1; - expected_system_default_qos.reliability = - RMW_QOS_POLICY_RELIABILITY_RELIABLE; - expected_system_default_qos.durability = - RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; - expected_system_default_qos.avoid_ros_namespace_conventions = - false; + rmw_qos_profile_t expected_system_default_qos = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + false}; + parameters.push_back({ + rmw_qos_profile_system_default, + expected_system_default_qos, + "publisher_system_default_qos"}); + } else if (!std::string("rmw_opensplice_cpp").compare(RMW_IMPLEMENTATION_STR)) { + rmw_qos_profile_t expected_system_default_qos = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + false}; + parameters.push_back({ + rmw_qos_profile_system_default, + expected_system_default_qos, + "publisher_system_default_qos"}); } - parameters.push_back({ - rmw_qos_profile_system_default, - expected_system_default_qos, - "publisher_system_default_qos"}); #endif return parameters; } From 2d7bb57564f039884297842305e1c53eec43cd71 Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Wed, 27 Mar 2019 12:38:00 -0300 Subject: [PATCH 6/8] Add system default test for connext Signed-off-by: ivanpauno --- rcl/test/rcl/test_get_actual_qos.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/rcl/test/rcl/test_get_actual_qos.cpp b/rcl/test/rcl/test_get_actual_qos.cpp index 589dc198a..bb3c979b1 100644 --- a/rcl/test/rcl/test_get_actual_qos.cpp +++ b/rcl/test/rcl/test_get_actual_qos.cpp @@ -181,8 +181,9 @@ get_parameters() }); #ifdef RMW_IMPLEMENTATION_STR - if (!std::string("rmw_fastrtps_cpp").compare(RMW_IMPLEMENTATION_STR) || - !std::string("rmw_fastrtps_dynamic_cpp").compare(RMW_IMPLEMENTATION_STR)) + std::string rmw_implementation_str = RMW_IMPLEMENTATION_STR; + if (!rmw_implementation_str.compare("rmw_fastrtps_cpp") || + !rmw_implementation_str.compare("rmw_fastrtps_dynamic_cpp")) { rmw_qos_profile_t expected_system_default_qos = { RMW_QOS_POLICY_HISTORY_KEEP_LAST, @@ -194,7 +195,10 @@ get_parameters() rmw_qos_profile_system_default, expected_system_default_qos, "publisher_system_default_qos"}); - } else if (!std::string("rmw_opensplice_cpp").compare(RMW_IMPLEMENTATION_STR)) { + } else if (!rmw_implementation_str.compare("rmw_opensplice_cpp") || + !rmw_implementation_str.compare("rmw_connext_cpp") || + !rmw_implementation_str.compare("rmw_connext_dynamic_cpp")) + { rmw_qos_profile_t expected_system_default_qos = { RMW_QOS_POLICY_HISTORY_KEEP_LAST, 1, From c91a27b676c36aa78ccc82ac4b90ba3696979199 Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Thu, 28 Mar 2019 12:03:08 -0300 Subject: [PATCH 7/8] Corrected with PR comments Signed-off-by: ivanpauno --- rcl/include/rcl/publisher.h | 13 +++---- rcl/src/rcl/publisher.c | 36 +++++++++---------- rcl/test/rcl/test_get_actual_qos.cpp | 52 ++++++++++++++-------------- 3 files changed, 47 insertions(+), 54 deletions(-) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index c5a62c980..61b89e741 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -473,6 +473,7 @@ rcl_publisher_get_subscription_count( * depends on the underlying rmw implementation. * If the underlying setting in use can't be represented in ROS terms, * it will be set to RMW_*_UNKNOWN. + * The returned struct is only valid as long as the rcl_publisher_t is valid. * *
* Attribute | Adherence @@ -483,18 +484,12 @@ rcl_publisher_get_subscription_count( * Lock-Free | Yes * * \param[in] publisher pointer to the rcl publisher - * \param[out] qos the actual qos settings - * \return `RCL_RET_OK` if the count was retrieved, or - * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or - * \return `RCL_RET_PUBLISHER_INVALID` if the publisher is invalid, or - * \return `RCL_RET_ERROR` if an unspecified error occurs. + * \return qos struct if successful, otherwise `NULL` */ RCL_PUBLIC RCL_WARN_UNUSED -rmw_ret_t -rcl_publisher_get_actual_qos( - const rcl_publisher_t * publisher, - rmw_qos_profile_t * qos); +const rmw_qos_profile_t * +rcl_publisher_get_actual_qos(const rcl_publisher_t * publisher); #ifdef __cplusplus } diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 84f5adeb8..4491612b5 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -35,6 +35,7 @@ extern "C" typedef struct rcl_publisher_impl_t { rcl_publisher_options_t options; + rmw_qos_profile_t actual_qos; rcl_context_t * context; rmw_publisher_t * rmw_handle; } rcl_publisher_impl_t; @@ -171,6 +172,17 @@ rcl_publisher_init( &(options->qos)); RCL_CHECK_FOR_NULL_WITH_MSG(publisher->impl->rmw_handle, rmw_get_error_string().str, goto fail); + // get actual qos, and store it + rmw_ret = rmw_publisher_get_actual_qos( + publisher->impl->rmw_handle, + &publisher->impl->actual_qos); + if (RMW_RET_OK != rmw_ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + ret = RCL_RET_ERROR; + goto fail; + } + publisher->impl->actual_qos.avoid_ros_namespace_conventions = + options->qos.avoid_ros_namespace_conventions; // options publisher->impl->options = *options; RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized"); @@ -349,27 +361,13 @@ rcl_publisher_get_subscription_count( return RCL_RET_OK; } -rmw_ret_t -rcl_publisher_get_actual_qos( - const rcl_publisher_t * publisher, - rmw_qos_profile_t * qos) +const rmw_qos_profile_t * +rcl_publisher_get_actual_qos(const rcl_publisher_t * publisher) { - if (!rcl_publisher_is_valid(publisher)) { - return RCL_RET_PUBLISHER_INVALID; - } - RCL_CHECK_ARGUMENT_FOR_NULL(qos, RCL_RET_INVALID_ARGUMENT); - - rmw_ret_t ret = rmw_publisher_get_actual_qos(publisher->impl->rmw_handle, - qos); - - if (ret != RMW_RET_OK) { - RCL_SET_ERROR_MSG(rmw_get_error_string().str); - return rcl_convert_rmw_ret_to_rcl_ret(ret); + if (!rcl_publisher_is_valid_except_context(publisher)) { + return NULL; } - qos->avoid_ros_namespace_conventions = - publisher->impl->options.qos.avoid_ros_namespace_conventions; - - return RCL_RET_OK; + return &publisher->impl->actual_qos; } #ifdef __cplusplus diff --git a/rcl/test/rcl/test_get_actual_qos.cpp b/rcl/test/rcl/test_get_actual_qos.cpp index bb3c979b1..aa91598ef 100644 --- a/rcl/test/rcl/test_get_actual_qos.cpp +++ b/rcl/test/rcl/test_get_actual_qos.cpp @@ -22,17 +22,15 @@ #include "rcl/publisher.h" #include "rcutils/logging_macros.h" +#include "rcutils/macros.h" #include "test_msgs/msg/primitives.h" #include "test_msgs/srv/primitives.h" -#define STRINGIFY_(x) #x -#define STRINGIFY(x) STRINGIFY_(x) - #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX # define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) -# define RMW_IMPLEMENTATION_STR STRINGIFY(RMW_IMPLEMENTATION) +# define RMW_IMPLEMENTATION_STR RCUTILS_STRINGIFY(RMW_IMPLEMENTATION) #else # define CLASSNAME(NAME, SUFFIX) NAME #endif @@ -121,23 +119,23 @@ TEST_P_RMW(TestGetActualQoS, test_publisher_get_qos_settings) { EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_reset_error(); - rmw_qos_profile_t qos; - ret = rcl_publisher_get_actual_qos(&pub, &qos); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + const rmw_qos_profile_t * qos; + qos = rcl_publisher_get_actual_qos(&pub); + EXPECT_NE(nullptr, qos) << rcl_get_error_string().str; EXPECT_EQ( - qos.history, + qos->history, parameters.qos_expected.history); EXPECT_EQ( - qos.depth, + qos->depth, parameters.qos_expected.depth); EXPECT_EQ( - qos.reliability, + qos->reliability, parameters.qos_expected.reliability); EXPECT_EQ( - qos.durability, + qos->durability, parameters.qos_expected.durability); EXPECT_EQ( - qos.avoid_ros_namespace_conventions, + qos->avoid_ros_namespace_conventions, parameters.qos_expected.avoid_ros_namespace_conventions); ret = rcl_publisher_fini(&pub, this->node_ptr); @@ -195,20 +193,22 @@ get_parameters() rmw_qos_profile_system_default, expected_system_default_qos, "publisher_system_default_qos"}); - } else if (!rmw_implementation_str.compare("rmw_opensplice_cpp") || - !rmw_implementation_str.compare("rmw_connext_cpp") || - !rmw_implementation_str.compare("rmw_connext_dynamic_cpp")) - { - rmw_qos_profile_t expected_system_default_qos = { - RMW_QOS_POLICY_HISTORY_KEEP_LAST, - 1, - RMW_QOS_POLICY_RELIABILITY_RELIABLE, - RMW_QOS_POLICY_DURABILITY_VOLATILE, - false}; - parameters.push_back({ - rmw_qos_profile_system_default, - expected_system_default_qos, - "publisher_system_default_qos"}); + } else { + if (!rmw_implementation_str.compare("rmw_opensplice_cpp") || + !rmw_implementation_str.compare("rmw_connext_cpp") || + !rmw_implementation_str.compare("rmw_connext_dynamic_cpp")) + { + rmw_qos_profile_t expected_system_default_qos = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + false}; + parameters.push_back({ + rmw_qos_profile_system_default, + expected_system_default_qos, + "publisher_system_default_qos"}); + } } #endif return parameters; From 8c2cae289ea1596a1a0b2cf46f63b9cb3515ae4c Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Fri, 29 Mar 2019 17:41:39 -0300 Subject: [PATCH 8/8] Solve windows build problem Signed-off-by: ivanpauno --- rcl/test/rcl/test_get_actual_qos.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/rcl/test/rcl/test_get_actual_qos.cpp b/rcl/test/rcl/test_get_actual_qos.cpp index aa91598ef..e15659dd3 100644 --- a/rcl/test/rcl/test_get_actual_qos.cpp +++ b/rcl/test/rcl/test_get_actual_qos.cpp @@ -35,14 +35,16 @@ # define CLASSNAME(NAME, SUFFIX) NAME #endif +#define EXPAND(x) x #define TEST_FIXTURE_P_RMW(test_fixture_name) CLASSNAME(test_fixture_name, \ RMW_IMPLEMENTATION) -#define APPLY(macro, ...) macro(__VA_ARGS__) -#define TEST_P_RMW(test_case_name, test_name) APPLY(TEST_P, \ +#define APPLY(macro, ...) EXPAND(macro(__VA_ARGS__)) +#define TEST_P_RMW(test_case_name, test_name) \ + APPLY(TEST_P, \ CLASSNAME(test_case_name, RMW_IMPLEMENTATION), test_name) -#define INSTANTIATE_TEST_CASE_P_RMW(instance_name, test_case_name, ...) APPLY( \ - INSTANTIATE_TEST_CASE_P, instance_name, CLASSNAME(test_case_name, \ - RMW_IMPLEMENTATION), __VA_ARGS__) +#define INSTANTIATE_TEST_CASE_P_RMW(instance_name, test_case_name, ...) \ + EXPAND(APPLY(INSTANTIATE_TEST_CASE_P, instance_name, \ + CLASSNAME(test_case_name, RMW_IMPLEMENTATION), __VA_ARGS__)) /** * Parameterized test.