diff --git a/rcl/CHANGELOG.rst b/rcl/CHANGELOG.rst index 64e229e40..be6bfc260 100644 --- a/rcl/CHANGELOG.rst +++ b/rcl/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package rcl ^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.0 (2019-04-14) +------------------ +* Added more test cases for graph API + fix bug. (`#404 `_) +* Fixed missing include. (`#413 `_) +* Updated to use pedantic. (`#412 `_) +* Added function to get publisher actual qos settings. (`#406 `_) +* Refactored graph API docs. (`#401 `_) +* Updated to use ament_target_dependencies where possible. (`#400 `_) +* Fixed regression around fully qualified node name. (`#402 `_) +* Added function rcl_names_and_types_init. (`#403 `_) +* Fixed uninitialize sequence number of client. (`#395 `_) +* Added launch along with launch_testing as test dependencies. (`#393 `_) +* Set symbol visibility to hidden for rcl. (`#391 `_) +* Updated to split test_token to avoid compiler note. (`#392 `_) +* Dropped legacy launch API usage. (`#387 `_) +* Improved security directory lookup. (`#332 `_) +* Enforce non-null argv values on rcl_init(). (`#388 `_) +* Removed incorrect argument documentation. (`#361 `_) +* Changed error to warning for multiple loggers. (`#384 `_) +* Added rcl_node_get_fully_qualified_name. (`#255 `_) +* Updated rcl_remap_t to use the PIMPL pattern. (`#377 `_) +* Fixed documentation typo. (`#376 `_) +* Removed test circumvention now that a bug is fixed in rmw_opensplice. (`#368 `_) +* Updated to pass context to wait set, and fini rmw context. (`#373 `_) +* Updated to publish logs to Rosout. (`#350 `_) +* Contributors: AAlon, Dirk Thomas, Jacob Perron, M. M, Michael Carroll, Michel Hidalgo, Mikael Arguedas, Nick Burek, RARvolt, Ross Desmond, Sachin Suresh Bhat, Shane Loretz, William Woodall, ivanpauno + 0.6.4 (2019-01-11) ------------------ * Added method for accessing rmw_context from rcl_context (`#372 `_) diff --git a/rcl/include/rcl/publisher.h b/rcl/include/rcl/publisher.h index 61b89e741..f1fb67a94 100644 --- a/rcl/include/rcl/publisher.h +++ b/rcl/include/rcl/publisher.h @@ -290,6 +290,32 @@ rcl_ret_t rcl_publish_serialized_message( const rcl_publisher_t * publisher, const rcl_serialized_message_t * serialized_message); +/// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) +/** + * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, the creator of + * this publisher may manually call `assert_liveliness` at some point in time to signal to the rest + * of the system that this Node is still alive. + * This function must be called at least as often as the qos_profile's liveliness_lease_duration + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] publisher handle to the publisher that needs liveliness to be asserted + * \return `RCL_RET_OK` if the liveliness assertion was completed successfully, 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 +rcl_ret_t +rcl_publisher_assert_liveliness(const rcl_publisher_t * publisher); + /// Get the topic name for the publisher. /** * This function returns the publisher's internal topic name string. diff --git a/rcl/package.xml b/rcl/package.xml index cae0e9a61..39fb0337e 100644 --- a/rcl/package.xml +++ b/rcl/package.xml @@ -2,7 +2,7 @@ rcl - 0.6.4 + 0.7.0 The ROS client library common implementation. This package contains an API which builds on the ROS middleware API and is optionally built upon by the other ROS client libraries. diff --git a/rcl/src/rcl/graph.c b/rcl/src/rcl/graph.c index 574a7d0f2..f16efe4db 100644 --- a/rcl/src/rcl/graph.c +++ b/rcl/src/rcl/graph.c @@ -42,7 +42,7 @@ rcl_get_publisher_names_and_types_by_node( if (!rcl_node_is_valid(node)) { return RCL_RET_NODE_INVALID; } - RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node_namespace, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT); @@ -80,7 +80,7 @@ rcl_get_subscriber_names_and_types_by_node( if (!rcl_node_is_valid(node)) { return RCL_RET_NODE_INVALID; } - RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node_namespace, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT); @@ -117,7 +117,7 @@ rcl_get_service_names_and_types_by_node( if (!rcl_node_is_valid(node)) { return RCL_RET_NODE_INVALID; } - RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(node_namespace, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RCL_RET_INVALID_ARGUMENT); @@ -152,7 +152,7 @@ rcl_get_topic_names_and_types( if (!rcl_node_is_valid(node)) { return RCL_RET_NODE_INVALID; // error already set } - RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT); rmw_ret_t rmw_ret; rmw_ret = rmw_names_and_types_check_zero(topic_names_and_types); @@ -178,6 +178,7 @@ rcl_get_service_names_and_types( if (!rcl_node_is_valid(node)) { return RCL_RET_NODE_INVALID; // error already set } + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RCL_RET_INVALID_ARGUMENT); rmw_ret_t rmw_ret; rmw_ret = rmw_names_and_types_check_zero(service_names_and_types); @@ -200,6 +201,7 @@ rcl_names_and_types_init( rcl_allocator_t * allocator) { RCL_CHECK_ARGUMENT_FOR_NULL(names_and_types, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR(allocator, return RCL_RET_INVALID_ARGUMENT); rmw_ret_t rmw_ret = rmw_names_and_types_init(names_and_types, size, allocator); return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); } diff --git a/rcl/src/rcl/publisher.c b/rcl/src/rcl/publisher.c index 2d7d167ab..44db15711 100644 --- a/rcl/src/rcl/publisher.c +++ b/rcl/src/rcl/publisher.c @@ -270,6 +270,19 @@ rcl_publish_serialized_message( return RCL_RET_OK; } +rcl_ret_t +rcl_publisher_assert_liveliness(const rcl_publisher_t * publisher) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; // error already set + } + if (rmw_publisher_assert_liveliness(publisher->impl->rmw_handle) != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + const char * rcl_publisher_get_topic_name(const rcl_publisher_t * publisher) { diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 1da554a06..995a8f555 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -100,7 +100,7 @@ function(test_target_function) # TODO(mm318): why rmw_connext tests run much slower than rmw_fastrtps and rmw_opensplice tests elseif(rmw_implementation STREQUAL "rmw_connext_cpp") message(STATUS "Increasing test_graph${target_suffix} test timeout.") - set(AMENT_GTEST_ARGS TIMEOUT 90) + set(AMENT_GTEST_ARGS TIMEOUT 180) endif() rcl_add_custom_gtest(test_graph${target_suffix} SRCS rcl/test_graph.cpp diff --git a/rcl/test/rcl/test_events.cpp b/rcl/test/rcl/test_events.cpp index b50c3ec48..064cee9c0 100644 --- a/rcl/test/rcl/test_events.cpp +++ b/rcl/test/rcl/test_events.cpp @@ -40,7 +40,7 @@ constexpr seconds DEADLINE_PERIOD_IN_S(1); # 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() @@ -220,7 +220,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(ret, RCL_RET_OK) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ rcl_ret_t ret = rcl_wait_set_fini(&wait_set); diff --git a/rcl/test/rcl/test_get_actual_qos.cpp b/rcl/test/rcl/test_get_actual_qos.cpp index 269188bdc..bd455a89c 100644 --- a/rcl/test/rcl/test_get_actual_qos.cpp +++ b/rcl/test/rcl/test_get_actual_qos.cpp @@ -147,6 +147,35 @@ TEST_P_RMW(TestGetActualQoS, test_publisher_get_qos_settings) { rcl_reset_error(); } +static constexpr rmw_qos_profile_t non_default_qos_profile() +{ + rmw_qos_profile_t profile = rmw_qos_profile_default; + profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + profile.depth = 1000; + profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + profile.avoid_ros_namespace_conventions = true; + return profile; +} + +static constexpr rmw_qos_profile_t expected_fastrtps_default_qos_profile() +{ + rmw_qos_profile_t profile = rmw_qos_profile_default; + profile.depth = 1; + profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + return profile; +} + +static constexpr rmw_qos_profile_t expected_system_default_qos_profile() +{ + rmw_qos_profile_t profile = rmw_qos_profile_default; + profile.depth = 1; + profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + return profile; +} + std::vector get_parameters() { @@ -164,28 +193,8 @@ get_parameters() Test with non-default settings. */ { - { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1000, - RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, - RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, - RMW_QOS_LIFESPAN_DEFAULT, - true - }, - { - RMW_QOS_POLICY_HISTORY_KEEP_ALL, - 1000, - RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, - RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, - RMW_QOS_LIFESPAN_DEFAULT, - true - }, + non_default_qos_profile(), + non_default_qos_profile(), "publisher_non_default_qos" } }); @@ -195,35 +204,17 @@ get_parameters() if (rmw_implementation_str == "rmw_fastrtps_cpp" || rmw_implementation_str == "rmw_fastrtps_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_TRANSIENT_LOCAL, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, - RMW_QOS_LIFESPAN_DEFAULT, - false}; + rmw_qos_profile_t expected_system_default_qos = expected_fastrtps_default_qos_profile(); parameters.push_back({ rmw_qos_profile_system_default, expected_system_default_qos, "publisher_system_default_qos"}); } else { if (rmw_implementation_str == "rmw_connext_cpp" || - rmw_implementation_str == "rmw_connext_dynamic_cpp" || - rmw_implementation_str == "rmw_opensplice_cpp") + rmw_implementation_str == "rmw_connext_dynamic_cpp" || + rmw_implementation_str == "rmw_opensplice_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, - RMW_QOS_DEADLINE_DEFAULT, - RMW_QOS_LIFESPAN_DEFAULT, - RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, - RMW_QOS_LIFESPAN_DEFAULT, - false}; + rmw_qos_profile_t expected_system_default_qos = expected_system_default_qos_profile(); parameters.push_back({ rmw_qos_profile_system_default, expected_system_default_qos, @@ -231,6 +222,7 @@ get_parameters() } } #endif + return parameters; } diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index 29eb1c81e..e0cf42127 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -131,7 +131,7 @@ class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test /* Test the rcl_get_topic_names_and_types and rcl_destroy_topic_names_and_types functions. * - * This does not test content of the rcl_topic_names_and_types_t structure. + * This does not test content of the rcl_names_and_types_t structure. */ TEST_F( CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), @@ -139,6 +139,8 @@ TEST_F( ) { rcl_ret_t ret; rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); rcl_names_and_types_t tnat {}; rcl_node_t zero_node = rcl_get_zero_initialized_node(); // invalid node @@ -155,6 +157,9 @@ TEST_F( ret = rcl_get_topic_names_and_types(this->node_ptr, nullptr, false, &tnat); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; rcl_reset_error(); + ret = rcl_get_topic_names_and_types(this->node_ptr, &zero_allocator, false, &tnat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); // invalid topic_names_and_types ret = rcl_get_topic_names_and_types(this->node_ptr, &allocator, false, nullptr); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; @@ -170,6 +175,52 @@ TEST_F( EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } +/* Test the rcl_get_service_names_and_types function. + * + * This does not test content of the rcl_names_and_types_t structure. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_get_service_names_and_types +) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); + rcl_names_and_types_t tnat {}; + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + // invalid node + ret = rcl_get_service_names_and_types(nullptr, &allocator, &tnat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types(&zero_node, &allocator, &tnat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types(this->old_node_ptr, &allocator, &tnat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid allocator + ret = rcl_get_service_names_and_types(this->node_ptr, nullptr, &tnat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types(this->node_ptr, &zero_allocator, &tnat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid service_names_and_types + ret = rcl_get_service_names_and_types(this->node_ptr, &allocator, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid argument to rcl_destroy_service_names_and_types + ret = rcl_names_and_types_fini(nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // valid calls + ret = rcl_get_service_names_and_types(this->node_ptr, &allocator, &tnat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_names_and_types_fini(&tnat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + /* Test the rcl_names_and_types_init function. */ TEST_F( @@ -178,6 +229,8 @@ TEST_F( ) { rcl_ret_t ret; rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); // invalid names and types ret = rcl_names_and_types_init(nullptr, 10, &allocator); @@ -187,6 +240,9 @@ TEST_F( ret = rcl_names_and_types_init(&nat, 10, nullptr); EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; rcl_reset_error(); + ret = rcl_names_and_types_init(&nat, 10, &zero_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); // zero size ret = rcl_names_and_types_init(&nat, 0, &allocator); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -205,9 +261,261 @@ TEST_F( EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } +/* Test the rcl_get_publisher_names_and_types_by_node function. + * + * This does not test content of the response. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_get_publisher_names_and_types_by_node +) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + const char * unknown_node_name = "/test_rcl_get_publisher_names_and_types_by_node"; + // const char * unknown_node_ns = "/test/namespace"; + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + // invalid node + ret = rcl_get_publisher_names_and_types_by_node( + nullptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_publisher_names_and_types_by_node( + &zero_node, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_publisher_names_and_types_by_node( + this->old_node_ptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid allocator + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, nullptr, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &zero_allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid names + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, nullptr, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, nullptr, &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // test valid strings with invalid node names + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, "", "", &nat); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, "_InvalidNodeName", "", &nat); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // TODO(jacobperron): This succeeds, but should fail due to invalid namespace + // ret = rcl_get_publisher_names_and_types_by_node( + // this->node_ptr, &allocator, false, this->test_graph_node_name, "_!invalidNs", &nat); + // EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + // rcl_reset_error(); + // invalid names and types + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "", nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // unknown node name + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, unknown_node_name, "", &nat); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // unknown node namespace + // TODO(jacobperron): This succeeds, but should fail due to invalid namespace + // ret = rcl_get_publisher_names_and_types_by_node( + // this->node_ptr, &allocator, false, this->test_graph_node_name, unknown_node_ns, &nat); + // EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + // rcl_reset_error(); + // valid call + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +/* Test the rcl_get_subscriber_names_and_types_by_node function. + * + * This does not test content of the response. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriber_names_and_types_by_node +) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + const char * unknown_node_name = "/test_rcl_get_subscriber_names_and_types_by_node"; + // const char * unknown_node_ns = "/test/namespace"; + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + // invalid node + ret = rcl_get_subscriber_names_and_types_by_node( + nullptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_subscriber_names_and_types_by_node( + &zero_node, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_subscriber_names_and_types_by_node( + this->old_node_ptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid allocator + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, nullptr, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &zero_allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid names + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, nullptr, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, nullptr, &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // test valid strings with invalid node names + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, "", "", &nat); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, "_InvalidNodeName", "", &nat); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // TODO(jacobperron): This succeeds, but should fail due to invalid namespace + // ret = rcl_get_subscriber_names_and_types_by_node( + // this->node_ptr, &allocator, false, this->test_graph_node_name, "_!invalidNs", &nat); + // EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + // rcl_reset_error(); + // invalid names and types + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "", nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // unknown node name + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, unknown_node_name, "", &nat); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // unknown node namespace + // TODO(jacobperron): This succeeds, but should fail due to invalid namespace + // ret = rcl_get_subscriber_names_and_types_by_node( + // this->node_ptr, &allocator, false, this->test_graph_node_name, unknown_node_ns, &nat); + // EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + // rcl_reset_error(); + // valid call + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +/* Test the rcl_get_service_names_and_types_by_node function. + * + * This does not test content of the response. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_get_service_names_and_types_by_node +) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + const char * unknown_node_name = "/test_rcl_get_service_names_and_types_by_node"; + // const char * unknown_node_ns = "/test/namespace"; + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + // invalid node + ret = rcl_get_service_names_and_types_by_node( + nullptr, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types_by_node( + &zero_node, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types_by_node( + this->old_node_ptr, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid allocator + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, nullptr, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &zero_allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid names + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, nullptr, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, nullptr, &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // test valid strings with invalid node names + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, "", "", &nat); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, "_InvalidNodeName", "", &nat); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // TODO(jacobperron): This succeeds, but should fail due to invalid namespace + // ret = rcl_get_service_names_and_types_by_node( + // this->node_ptr, &allocator, false, this->test_graph_node_name, "_!invalidNs", &nat); + // EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + // rcl_reset_error(); + // invalid names and types + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, "", nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // unknown node name + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, unknown_node_name, "", &nat); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // unknown node namespace + // TODO(jacobperron): This succeeds, but should fail due to invalid namespace + // ret = rcl_get_service_names_and_types_by_node( + // this->node_ptr, &allocator, this->test_graph_node_name, unknown_node_ns, &nat); + // EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + // rcl_reset_error(); + // valid call + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + /* Test the rcl_count_publishers function. * - * This does not test content the response. + * This does not test content of the response. */ TEST_F( CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), @@ -244,7 +552,7 @@ TEST_F( /* Test the rcl_count_subscribers function. * - * This does not test content the response. + * This does not test content of the response. */ TEST_F( CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), diff --git a/rcl/test/rcl/test_timer.cpp b/rcl/test/rcl/test_timer.cpp index 3ab86508e..71d6f1545 100644 --- a/rcl/test/rcl/test_timer.cpp +++ b/rcl/test/rcl/test_timer.cpp @@ -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)) << diff --git a/rcl_action/CHANGELOG.rst b/rcl_action/CHANGELOG.rst index fca6cceed..0d01eca78 100644 --- a/rcl_action/CHANGELOG.rst +++ b/rcl_action/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package rcl_action ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.0 (2019-04-14) +------------------ +* Added Action graph API (`#411 `_) +* Updated to use ament_target_dependencies where possible. (`#400 `_) +* Fixed typo in Doxyfile. (`#398 `_) +* Updated tests to use separated action types. (`#340 `_) +* Fixed minor documentation issues. (`#397 `_) +* Set symbol visibility to hidden for rcl. (`#391 `_) +* Fixed rcl_action documentation. (`#380 `_) +* Removed now unused test executable . (`#382 `_) +* Removed unused action server option 'clock_type'. (`#382 `_) +* Set error message when there is an invalid goal transition. (`#382 `_) +* Updated to pass context to wait set, and fini rmw context (`#373 `_) +* Contributors: Dirk Thomas, Jacob Perron, Sachin Suresh Bhat, William Woodall, ivanpauno + 0.6.4 (2019-01-11) ------------------ * Added parentheses around use of zerouuid macro (`#371 `_) diff --git a/rcl_action/CMakeLists.txt b/rcl_action/CMakeLists.txt index f16868b54..ffb1fa65b 100644 --- a/rcl_action/CMakeLists.txt +++ b/rcl_action/CMakeLists.txt @@ -36,6 +36,7 @@ set(rcl_action_sources src/${PROJECT_NAME}/action_server.c src/${PROJECT_NAME}/goal_handle.c src/${PROJECT_NAME}/goal_state_machine.c + src/${PROJECT_NAME}/graph.c src/${PROJECT_NAME}/names.c src/${PROJECT_NAME}/types.c ) @@ -91,7 +92,11 @@ if(BUILD_TESTING) target_link_libraries(test_action_client ${PROJECT_NAME} ) - ament_target_dependencies(test_action_client "osrf_testing_tools_cpp" "rcl" "test_msgs") + ament_target_dependencies(test_action_client + "osrf_testing_tools_cpp" + "rcl" + "test_msgs" + ) endif() # get the rmw implementations ahead of time @@ -122,6 +127,7 @@ if(BUILD_TESTING) ${PROJECT_NAME} ) ament_target_dependencies(${target}${target_suffix} + "osrf_testing_tools_cpp" "rcl" "test_msgs" ) @@ -146,6 +152,13 @@ if(BUILD_TESTING) "test/rcl_action/test_action_communication.cpp") custom_test_c(test_action_interaction "test/rcl_action/test_action_interaction.cpp") + + # TODO(jacobperron): Graph tests fail with opensplice. Re-enable after resolving + # https://github.com/ros2/ros2/issues/677 + if(NOT rmw_implementation STREQUAL "rmw_opensplice_cpp") + custom_test_c(test_graph + "test/rcl_action/test_graph.cpp") + endif() endmacro() call_for_each_rmw_implementation(targets) diff --git a/rcl_action/include/rcl_action/default_qos.h b/rcl_action/include/rcl_action/default_qos.h index 18ed5a60d..c1952b708 100644 --- a/rcl_action/include/rcl_action/default_qos.h +++ b/rcl_action/include/rcl_action/default_qos.h @@ -29,10 +29,10 @@ static const rmw_qos_profile_t rcl_action_qos_profile_status_default = 1, RMW_QOS_POLICY_RELIABILITY_RELIABLE, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, - {0, 0}, - {0, 0}, + RMW_QOS_DEADLINE_DEFAULT, + RMW_QOS_LIFESPAN_DEFAULT, RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, - {0, 0}, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, false }; diff --git a/rcl_action/include/rcl_action/graph.h b/rcl_action/include/rcl_action/graph.h new file mode 100644 index 000000000..7d4fc82e6 --- /dev/null +++ b/rcl_action/include/rcl_action/graph.h @@ -0,0 +1,162 @@ +// 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. +// 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. + +#ifndef RCL_ACTION__GRAPH_H_ +#define RCL_ACTION__GRAPH_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/graph.h" +#include "rcl/node.h" + +#include "rcl_action/visibility_control.h" + +/// Get a list of action names and types for action clients associated with a node. +/** + * The `node` parameter must point to a valid node. + * + * The `action_names_and_types` parameter must be allocated and zero initialized. + * This function allocates memory for the returned list of names and types and so it is the + * callers responsibility to pass `action_names_and_types` to rcl_names_and_types_fini() + * when it is no longer needed. + * Failing to do so will result in leaked memory. + * + * The returned names are not automatically remapped by this function. + * Attempting to create action clients or action servers with names returned by this function may + * not result in the desired action name depending on the remap rules in use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator allocator for allocating space for strings + * \param[in] node_name the node name of the actions to return + * \param[in] node_namespace the node namespace of the actions to return + * \param[out] action_names_and_types list of action names and their types + * \return `RCL_RET_OK` if the query was successful, or + * \return `RCL_RET_NODE_INVALID` if the node is invalid, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_get_client_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * action_names_and_types); + +/// Get a list of action names and types for action servers associated with a node. +/** + * This function returns a list of action names and types for action servers associated with + * the provided node name. + * + * The `node` parameter must point to a valid node. + * + * The `action_names_and_types` parameter must be allocated and zero initialized. + * This function allocates memory for the returned list of names and types and so it is the + * callers responsibility to pass `action_names_and_types` to rcl_names_and_types_fini() + * when it is no longer needed. + * Failing to do so will result in leaked memory. + * + * The returned names are not automatically remapped by this function. + * Attempting to create action clients or action servers with names returned by this function may + * not result in the desired action name depending on the remap rules in use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator allocator for allocating space for strings + * \param[in] node_name the node name of the actions to return + * \param[in] node_namespace the node namespace of the actions to return + * \param[out] action_names_and_types list of action names and their types + * \return `RCL_RET_OK` if the query was successful, or + * \return `RCL_RET_NODE_INVALID` if the node is invalid, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_get_server_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * action_names_and_types); + +/// Return a list of action names and their types. +/** + * This function returns a list of action names and types in the ROS graph. + * + * The `node` parameter must point to a valid node. + * + * The `action_names_and_types` parameter must be allocated and zero initialized. + * This function allocates memory for the returned list of names and types and so it is the + * callers responsibility to pass `action_names_and_types` to rcl_names_and_types_fini() + * when it is no longer needed. + * Failing to do so will result in leaked memory. + * + * The returned names are not automatically remapped by this function. + * Attempting to create action clients or action servers with names returned by this function may + * not result in the desired action name depending on the remap rules in use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator allocator for allocating space for strings + * \param[out] action_names_and_types list of action names and types + * \return `RCL_RET_OK` if the query was successful, or + * \return `RCL_RET_NODE_INVALID` if the node is invalid, or + * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_ACTION_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_action_get_names_and_types( + const rcl_node_t * node, + rcl_allocator_t * allocator, + rcl_names_and_types_t * action_names_and_types); + +#ifdef __cplusplus +} +#endif + +#endif // RCL_ACTION__GRAPH_H_ diff --git a/rcl_action/include/rcl_action/rcl_action.h b/rcl_action/include/rcl_action/rcl_action.h index f99130eac..c7a33bb2d 100644 --- a/rcl_action/include/rcl_action/rcl_action.h +++ b/rcl_action/include/rcl_action/rcl_action.h @@ -52,6 +52,7 @@ extern "C" #include "rcl_action/default_qos.h" #include "rcl_action/goal_handle.h" #include "rcl_action/goal_state_machine.h" +#include "rcl_action/graph.h" #include "rcl_action/types.h" #include "rcl_action/wait.h" diff --git a/rcl_action/package.xml b/rcl_action/package.xml index 0af39632e..6a6d8378a 100644 --- a/rcl_action/package.xml +++ b/rcl_action/package.xml @@ -2,7 +2,7 @@ rcl_action - 0.6.4 + 0.7.0 Package containing a C-based ROS action implementation Jacob Perron Apache License 2.0 diff --git a/rcl_action/src/rcl_action/graph.c b/rcl_action/src/rcl_action/graph.c new file mode 100644 index 000000000..ce8474234 --- /dev/null +++ b/rcl_action/src/rcl_action/graph.c @@ -0,0 +1,221 @@ +// 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. +// 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/graph.h" +#include "rcl/node.h" +#include "rcutils/strdup.h" + +#include "rcl_action/graph.h" + +static +rcl_ret_t +_filter_action_names( + rcl_names_and_types_t * topic_names_and_types, + rcl_allocator_t * allocator, + rcl_names_and_types_t * action_names_and_types) +{ + assert(topic_names_and_types); + assert(allocator); + assert(action_names_and_types); + + // Assumption: actions provide a topic name with the suffix "/_action/feedback" + // and it has type with the suffix "_FeedbackMessage" + const char * action_name_identifier = "/_action/feedback"; + const char * action_type_identifier = "_FeedbackMessage"; + + rcl_ret_t ret; + const size_t num_names = topic_names_and_types->names.size; + char ** names = topic_names_and_types->names.data; + + // Count number of actions to determine how much memory to allocate + size_t num_actions = 0u; + for (size_t i = 0u; i < num_names; ++i) { + const char * identifier_index = strstr(names[i], action_name_identifier); + if (identifier_index && strlen(identifier_index) == strlen(action_name_identifier)) { + ++num_actions; + } + } + + if (0u == num_actions) { + return RCL_RET_OK; + } + + ret = rcl_names_and_types_init(action_names_and_types, num_actions, allocator); + if (RCL_RET_OK != ret) { + return ret; + } + + ret = RCL_RET_OK; + + // Prune names/types that are not actions (ie. do not contain the suffix) + const size_t suffix_len = strlen(action_name_identifier); + size_t j = 0u; + for (size_t i = 0u; i < num_names; ++i) { + const char * identifier_index = strstr(names[i], action_name_identifier); + if (identifier_index && strlen(identifier_index) == strlen(action_name_identifier)) { + const size_t action_name_len = strlen(names[i]) - suffix_len; + char * action_name = rcutils_strndup(names[i], action_name_len, *allocator); + if (!action_name) { + RCL_SET_ERROR_MSG("Failed to allocate memory for action name"); + ret = RCL_RET_BAD_ALLOC; + break; + } + + action_names_and_types->names.data[j] = action_name; + + // Allocate storage for type list + rcutils_ret_t rcutils_ret = rcutils_string_array_init( + &action_names_and_types->types[j], + topic_names_and_types->types[i].size, + allocator); + if (RCUTILS_RET_OK != rcutils_ret) { + RCL_SET_ERROR_MSG(rcutils_get_error_string().str); + ret = RCL_RET_BAD_ALLOC; + break; + } + + // Populate types list + for (size_t k = 0u; k < topic_names_and_types->types[i].size; ++k) { + char * type_name = topic_names_and_types->types[i].data[k]; + size_t action_type_len = strlen(type_name); + // Trim type name suffix + const size_t type_suffix_len = strlen(action_type_identifier); + const char * type_identifier_index = strstr(type_name, action_type_identifier); + if (type_identifier_index && + strlen(type_identifier_index) == strlen(action_type_identifier)) + { + action_type_len = strlen(type_name) - type_suffix_len; + } + // Copy name to output struct + char * action_type_name = rcutils_strndup(type_name, action_type_len, *allocator); + if (!action_type_name) { + RCL_SET_ERROR_MSG("Failed to allocate memory for action type"); + ret = RCL_RET_BAD_ALLOC; + break; + } + action_names_and_types->types[j].data[k] = action_type_name; + } + ++j; + } + } + + // Cleanup if there is an error + if (RCL_RET_OK != ret) { + rcl_ret_t fini_ret = rcl_names_and_types_fini(action_names_and_types); + (void)fini_ret; // Error already set + } + + return ret; +} + +rcl_ret_t +rcl_action_get_client_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * action_names_and_types) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(action_names_and_types, RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret; + rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); + ret = rcl_get_subscriber_names_and_types_by_node( + node, allocator, false, node_name, node_namespace, &topic_names_and_types); + if (RCL_RET_OK != ret) { + return ret; + } + + ret = _filter_action_names( + &topic_names_and_types, + allocator, + action_names_and_types); + + rcl_ret_t nat_fini_ret = rcl_names_and_types_fini(&topic_names_and_types); + if (RCL_RET_OK != nat_fini_ret) { + ret = rcl_names_and_types_fini(action_names_and_types); + return nat_fini_ret; + } + return ret; +} + +rcl_ret_t +rcl_action_get_server_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * action_names_and_types) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(action_names_and_types, RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret; + rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); + ret = rcl_get_publisher_names_and_types_by_node( + node, allocator, false, node_name, node_namespace, &topic_names_and_types); + if (RCL_RET_OK != ret) { + return ret; + } + + ret = _filter_action_names( + &topic_names_and_types, + allocator, + action_names_and_types); + + rcl_ret_t nat_fini_ret = rcl_names_and_types_fini(&topic_names_and_types); + if (RCL_RET_OK != nat_fini_ret) { + ret = rcl_names_and_types_fini(action_names_and_types); + return nat_fini_ret; + } + return ret; +} + +rcl_ret_t +rcl_action_get_names_and_types( + const rcl_node_t * node, + rcl_allocator_t * allocator, + rcl_names_and_types_t * action_names_and_types) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(action_names_and_types, RCL_RET_INVALID_ARGUMENT); + rcl_names_and_types_t topic_names_and_types = rcl_get_zero_initialized_names_and_types(); + rcl_ret_t ret = rcl_get_topic_names_and_types(node, allocator, false, &topic_names_and_types); + if (RCL_RET_OK != ret) { + return ret; + } + + ret = _filter_action_names( + &topic_names_and_types, + allocator, + action_names_and_types); + + rcl_ret_t nat_fini_ret = rcl_names_and_types_fini(&topic_names_and_types); + if (RCL_RET_OK != nat_fini_ret) { + ret = rcl_names_and_types_fini(action_names_and_types); + return nat_fini_ret; + } + return ret; +} + +#ifdef __cplusplus +} +#endif diff --git a/rcl_action/test/rcl_action/test_graph.cpp b/rcl_action/test/rcl_action/test_graph.cpp new file mode 100644 index 000000000..c35ac5baa --- /dev/null +++ b/rcl_action/test/rcl_action/test_graph.cpp @@ -0,0 +1,563 @@ +// 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. +// 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_action/action_client.h" +#include "rcl_action/action_server.h" +#include "rcl_action/graph.h" + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" + +#include "test_msgs/action/fibonacci.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestActionGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator; + rcl_context_t old_context; + rcl_context_t context; + rcl_node_t old_node; + rcl_node_t node; + rcl_node_t zero_node; + const char * test_graph_node_name = "test_action_graph_node"; + const char * test_graph_old_node_name = "test_action_graph_old_node_name"; + + void SetUp() + { + rcl_ret_t ret; + this->zero_node = rcl_get_zero_initialized_node(); + this->zero_allocator = static_cast(rcutils_get_zero_initialized_allocator()); + + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, this->allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->old_context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &this->old_context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->old_node = rcl_get_zero_initialized_node(); + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init( + &this->old_node, this->test_graph_old_node_name, "", &this->old_context, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(&this->old_context); // after this, the old_node should be invalid + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &this->context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&this->node, test_graph_node_name, "", &this->context, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret; + ret = rcl_node_fini(&this->old_node); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(&this->node); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_shutdown(&this->context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(&this->context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(&this->old_context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +TEST_F( + CLASSNAME(TestActionGraphFixture, RMW_IMPLEMENTATION), + test_action_get_client_names_and_types_by_node) +{ + rcl_ret_t ret; + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + + // Invalid node + ret = rcl_action_get_client_names_and_types_by_node( + nullptr, &this->allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_action_get_client_names_and_types_by_node( + &this->zero_node, &this->allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_action_get_client_names_and_types_by_node( + &this->old_node, &this->allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid allocator + ret = rcl_action_get_client_names_and_types_by_node( + &this->node, nullptr, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_action_get_client_names_and_types_by_node( + &this->node, &this->zero_allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid node name + ret = rcl_action_get_client_names_and_types_by_node( + &this->node, &this->allocator, "_test_this_Isnot_a_valid_name", "", &nat); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Non-existent node + ret = rcl_action_get_client_names_and_types_by_node( + &this->node, &this->allocator, this->test_graph_old_node_name, "", &nat); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid names and types + ret = rcl_action_get_client_names_and_types_by_node( + &this->node, &this->allocator, this->test_graph_node_name, "", nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Valid call + ret = rcl_action_get_client_names_and_types_by_node( + &this->node, &this->allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_names_and_types_fini(&nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +TEST_F( + CLASSNAME(TestActionGraphFixture, RMW_IMPLEMENTATION), + test_action_get_server_names_and_types_by_node) +{ + rcl_ret_t ret; + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + + // Invalid node + ret = rcl_action_get_server_names_and_types_by_node( + nullptr, &this->allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_action_get_server_names_and_types_by_node( + &this->zero_node, &this->allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_action_get_server_names_and_types_by_node( + &this->old_node, &this->allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid allocator + ret = rcl_action_get_server_names_and_types_by_node( + &this->node, nullptr, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_action_get_server_names_and_types_by_node( + &this->node, &this->zero_allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid node name + ret = rcl_action_get_server_names_and_types_by_node( + &this->node, &this->allocator, "_test_this_Isnot_a_valid_name", "", &nat); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Non-existent node + ret = rcl_action_get_server_names_and_types_by_node( + &this->node, &this->allocator, this->test_graph_old_node_name, "", &nat); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid names and types + ret = rcl_action_get_server_names_and_types_by_node( + &this->node, &this->allocator, this->test_graph_node_name, "", nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Valid call + ret = rcl_action_get_server_names_and_types_by_node( + &this->node, &this->allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_names_and_types_fini(&nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +TEST_F( + CLASSNAME(TestActionGraphFixture, RMW_IMPLEMENTATION), + test_action_get_names_and_types) +{ + rcl_ret_t ret; + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + + // Invalid node + ret = rcl_action_get_names_and_types(nullptr, &this->allocator, &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_action_get_names_and_types(&this->zero_node, &this->allocator, &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_action_get_names_and_types(&this->old_node, &this->allocator, &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid allocator + ret = rcl_action_get_names_and_types(&this->node, nullptr, &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_action_get_names_and_types(&this->node, &this->zero_allocator, &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid names and types + ret = rcl_action_get_names_and_types(&this->node, &this->allocator, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Valid call + ret = rcl_action_get_names_and_types(&this->node, &this->allocator, &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_names_and_types_fini(&nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +/** + * Type define a get actions function. + */ +typedef std::function GetActionsFunc; + +/** + * Extend the TestActionGraphFixture with a multi-node fixture for node discovery and node-graph + * perspective. + */ +class TestActionGraphMultiNodeFixture : public CLASSNAME(TestActionGraphFixture, RMW_IMPLEMENTATION) +{ +public: + const char * remote_node_name = "remote_graph_node"; + const char * action_name = "/test_action_info_functions__"; + rcl_node_t remote_node; + rcl_context_t remote_context; + GetActionsFunc action_func, clients_by_node_func, servers_by_node_func; + + void SetUp() override + { + CLASSNAME(TestActionGraphFixture, RMW_IMPLEMENTATION) ::SetUp(); + + rcl_ret_t ret; + 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; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << + rcl_get_error_string().str; + }); + + this->remote_node = rcl_get_zero_initialized_node(); + rcl_node_options_t node_options = rcl_node_get_default_options(); + + this->remote_context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &this->remote_context); + + ret = rcl_node_init( + &this->remote_node, this->remote_node_name, "", &this->remote_context, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + action_func = std::bind(rcl_action_get_names_and_types, + std::placeholders::_1, + &this->allocator, + std::placeholders::_2); + clients_by_node_func = std::bind(rcl_action_get_client_names_and_types_by_node, + std::placeholders::_1, + &this->allocator, + this->remote_node_name, + "", + std::placeholders::_2); + servers_by_node_func = std::bind(rcl_action_get_server_names_and_types_by_node, + std::placeholders::_1, + &this->allocator, + this->remote_node_name, + "", + std::placeholders::_2); + WaitForAllNodesAlive(); + } + + void TearDown() override + { + CLASSNAME(TestActionGraphFixture, RMW_IMPLEMENTATION) ::TearDown(); + + rcl_ret_t ret; + ret = rcl_node_fini(&this->remote_node); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_shutdown(&this->remote_context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(&this->remote_context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void WaitForAllNodesAlive() + { + rcl_ret_t ret; + rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + ret = rcutils_string_array_fini(&node_names); + ASSERT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_string_array_fini(&node_namespaces); + ASSERT_EQ(RCUTILS_RET_OK, ret); + }); + // Wait for all 3 nodes to be discovered: remote_node, old_node, node + size_t attempts = 0u; + size_t max_attempts = 4u; + while (node_names.size < 3u) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + ret = rcl_get_node_names(&this->remote_node, allocator, &node_names, &node_namespaces); + ++attempts; + ASSERT_LE(attempts, max_attempts) << "Unable to attain all required nodes"; + } + } + + void WaitForActionCount( + GetActionsFunc func, + size_t expected_count, + std::chrono::milliseconds duration) + { + auto start_time = std::chrono::system_clock::now(); + auto curr_time = start_time; + + rcl_ret_t ret; + while ((curr_time - start_time) < duration) { + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + ret = func(&this->node, &nat); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + size_t action_count = nat.names.size; + ret = rcl_names_and_types_fini(&nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + if (action_count == expected_count) { + return; + } + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + curr_time = std::chrono::system_clock::now(); + } + } +}; + +// Note, this test could be affected by other communication on the same ROS domain +TEST_F(TestActionGraphMultiNodeFixture, test_action_get_names_and_types) { + rcl_ret_t ret; + // Create an action client + rcl_action_client_t action_client = rcl_action_get_zero_initialized_client(); + const rosidl_action_type_support_t * action_typesupport = + ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci); + const char * client_action_name = "/test_action_get_names_and_types_client_action_name"; + rcl_action_client_options_t action_client_options = rcl_action_client_get_default_options(); + ret = rcl_action_client_init( + &action_client, + &this->remote_node, + action_typesupport, + client_action_name, + &action_client_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_action_client_fini(&action_client, &this->remote_node)) << + rcl_get_error_string().str; + }); + + WaitForActionCount(action_func, 1u, std::chrono::seconds(1)); + + // Check that there is exactly one action name + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + ret = action_func(&this->node, &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(nat.names.size, 1u); + EXPECT_STREQ(nat.names.data[0], client_action_name); + ASSERT_EQ(nat.types[0].size, 1u); + EXPECT_STREQ(nat.types[0].data[0], "test_msgs/Fibonacci"); + + ret = rcl_names_and_types_fini(&nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Create an action server + rcl_action_server_t action_server = rcl_action_get_zero_initialized_server(); + rcl_clock_t clock; + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &this->allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string().str; + }); + const char * server_action_name = "/test_action_get_names_and_types_server_action_name"; + rcl_action_server_options_t action_server_options = rcl_action_server_get_default_options(); + ret = rcl_action_server_init( + &action_server, + &this->remote_node, + &clock, + action_typesupport, + server_action_name, + &action_server_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_action_server_fini(&action_server, &this->remote_node)) << + rcl_get_error_string().str; + }); + + WaitForActionCount(action_func, 2u, std::chrono::seconds(1)); + + ret = action_func(&this->node, &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(nat.names.size, 2u); + EXPECT_STREQ(nat.names.data[0], client_action_name); + EXPECT_STREQ(nat.names.data[1], server_action_name); + ASSERT_EQ(nat.types[0].size, 1u); + EXPECT_STREQ(nat.types[0].data[0], "test_msgs/Fibonacci"); + ASSERT_EQ(nat.types[1].size, 1u); + EXPECT_STREQ(nat.types[1].data[0], "test_msgs/Fibonacci"); + + ret = rcl_names_and_types_fini(&nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +// Note, this test could be affected by other communication on the same ROS domain +TEST_F(TestActionGraphMultiNodeFixture, test_action_get_server_names_and_types_by_node) { + rcl_ret_t ret; + // Create an action client + rcl_action_client_t action_client = rcl_action_get_zero_initialized_client(); + const rosidl_action_type_support_t * action_typesupport = + ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci); + rcl_action_client_options_t action_client_options = rcl_action_client_get_default_options(); + ret = rcl_action_client_init( + &action_client, + &this->remote_node, + action_typesupport, + this->action_name, + &action_client_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_action_client_fini(&action_client, &this->remote_node)) << + rcl_get_error_string().str; + }); + // Check that there are no action servers + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + ret = rcl_action_get_server_names_and_types_by_node( + &this->node, &this->allocator, this->remote_node_name, "", &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(nat.names.size, 0u); + + ret = rcl_names_and_types_fini(&nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Create an action server + rcl_action_server_t action_server = rcl_action_get_zero_initialized_server(); + rcl_clock_t clock; + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &this->allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string().str; + }); + rcl_action_server_options_t action_server_options = rcl_action_server_get_default_options(); + ret = rcl_action_server_init( + &action_server, + &this->remote_node, + &clock, + action_typesupport, + this->action_name, + &action_server_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_action_server_fini(&action_server, &this->remote_node)) << + rcl_get_error_string().str; + }); + + WaitForActionCount(servers_by_node_func, 1u, std::chrono::seconds(1)); + ret = servers_by_node_func(&this->node, &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(nat.names.size, 1u); + EXPECT_STREQ(nat.names.data[0], this->action_name); + ASSERT_EQ(nat.types[0].size, 1u); + EXPECT_STREQ(nat.types[0].data[0], "test_msgs/Fibonacci"); + + ret = rcl_names_and_types_fini(&nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +// Note, this test could be affected by other communication on the same ROS domain +TEST_F(TestActionGraphMultiNodeFixture, test_action_get_client_names_and_types_by_node) { + rcl_ret_t ret; + const rosidl_action_type_support_t * action_typesupport = + ROSIDL_GET_ACTION_TYPE_SUPPORT(test_msgs, Fibonacci); + // Create an action server + rcl_action_server_t action_server = rcl_action_get_zero_initialized_server(); + rcl_clock_t clock; + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &this->allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string().str; + }); + rcl_action_server_options_t action_server_options = rcl_action_server_get_default_options(); + ret = rcl_action_server_init( + &action_server, + &this->remote_node, + &clock, + action_typesupport, + this->action_name, + &action_server_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_action_server_fini(&action_server, &this->remote_node)) << + rcl_get_error_string().str; + }); + + // Check that there are no action clients + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + ret = rcl_action_get_client_names_and_types_by_node( + &this->node, &this->allocator, this->remote_node_name, "", &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(nat.names.size, 0u); + + ret = rcl_names_and_types_fini(&nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Create an action client + rcl_action_client_t action_client = rcl_action_get_zero_initialized_client(); + rcl_action_client_options_t action_client_options = rcl_action_client_get_default_options(); + ret = rcl_action_client_init( + &action_client, + &this->remote_node, + action_typesupport, + this->action_name, + &action_client_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_action_client_fini(&action_client, &this->remote_node)) << + rcl_get_error_string().str; + }); + + WaitForActionCount(clients_by_node_func, 1u, std::chrono::seconds(1)); + ret = clients_by_node_func(&this->node, &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(nat.names.size, 1u); + EXPECT_STREQ(nat.names.data[0], this->action_name); + ASSERT_EQ(nat.types[0].size, 1u); + EXPECT_STREQ(nat.types[0].data[0], "test_msgs/Fibonacci"); + + ret = rcl_names_and_types_fini(&nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} diff --git a/rcl_lifecycle/CHANGELOG.rst b/rcl_lifecycle/CHANGELOG.rst index a411dff54..c1ab7b477 100644 --- a/rcl_lifecycle/CHANGELOG.rst +++ b/rcl_lifecycle/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package rcl_lifecycle ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.0 (2019-04-14) +------------------ +* Updated to use ament_target_dependencies where possible. (`#400 `_) +* Set symbol visibility to hidden for rcl. (`#391 `_) +* Contributors: Sachin Suresh Bhat, ivanpauno + 0.6.4 (2019-01-11) ------------------ diff --git a/rcl_lifecycle/package.xml b/rcl_lifecycle/package.xml index a5bd1f6fc..3845b5a0a 100644 --- a/rcl_lifecycle/package.xml +++ b/rcl_lifecycle/package.xml @@ -2,7 +2,7 @@ rcl_lifecycle - 0.6.4 + 0.7.0 Package containing a C-based lifecycle implementation Karsten Knese Apache License 2.0 diff --git a/rcl_yaml_param_parser/CHANGELOG.rst b/rcl_yaml_param_parser/CHANGELOG.rst index 100b5cd1f..259b19527 100644 --- a/rcl_yaml_param_parser/CHANGELOG.rst +++ b/rcl_yaml_param_parser/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package rcl_yaml_param_parser ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.7.0 (2019-04-14) +------------------ +* Corrected bool reading from yaml files. (`#415 `_) +* Added launch along with launch_testing as test dependencies. (`#393 `_) +* Set symbol visibility to hidden for rcl. (`#391 `_) +* Contributors: Michel Hidalgo, Sachin Suresh Bhat, ivanpauno + 0.6.4 (2019-01-11) ------------------ diff --git a/rcl_yaml_param_parser/package.xml b/rcl_yaml_param_parser/package.xml index 8c659e7b6..e5e4ba209 100644 --- a/rcl_yaml_param_parser/package.xml +++ b/rcl_yaml_param_parser/package.xml @@ -2,7 +2,7 @@ rcl_yaml_param_parser - 0.6.4 + 0.7.0 Package containing various utility types and functions for C Anup Pemmaiah Apache License 2.0 diff --git a/rcl_yaml_param_parser/src/parser.c b/rcl_yaml_param_parser/src/parser.c index e308ee380..d6f41d20c 100644 --- a/rcl_yaml_param_parser/src/parser.c +++ b/rcl_yaml_param_parser/src/parser.c @@ -118,6 +118,7 @@ static rcl_ret_t replace_ns( static void * get_value( const char * const value, + yaml_scalar_style_t style, data_types_t * val_type, const rcl_allocator_t allocator); @@ -709,6 +710,7 @@ static rcl_ret_t add_val_to_string_arr( /// static void * get_value( const char * const value, + yaml_scalar_style_t style, data_types_t * val_type, const rcl_allocator_t allocator) { @@ -723,46 +725,50 @@ static void * get_value( } /// Check if it is bool - if ((0 == strncmp(value, "Y", strlen(value))) || - (0 == strncmp(value, "y", strlen(value))) || - (0 == strncmp(value, "yes", strlen(value))) || - (0 == strncmp(value, "Yes", strlen(value))) || - (0 == strncmp(value, "YES", strlen(value))) || - (0 == strncmp(value, "true", strlen(value))) || - (0 == strncmp(value, "True", strlen(value))) || - (0 == strncmp(value, "TRUE", strlen(value))) || - (0 == strncmp(value, "on", strlen(value))) || - (0 == strncmp(value, "On", strlen(value))) || - (0 == strncmp(value, "ON", strlen(value)))) + if (style != YAML_SINGLE_QUOTED_SCALAR_STYLE && + style != YAML_DOUBLE_QUOTED_SCALAR_STYLE) { - *val_type = DATA_TYPE_BOOL; - ret_val = allocator.zero_allocate(1U, sizeof(bool), allocator.state); - if (NULL == ret_val) { - return NULL; + if ((0 == strcmp(value, "Y")) || + (0 == strcmp(value, "y")) || + (0 == strcmp(value, "yes")) || + (0 == strcmp(value, "Yes")) || + (0 == strcmp(value, "YES")) || + (0 == strcmp(value, "true")) || + (0 == strcmp(value, "True")) || + (0 == strcmp(value, "TRUE")) || + (0 == strcmp(value, "on")) || + (0 == strcmp(value, "On")) || + (0 == strcmp(value, "ON"))) + { + *val_type = DATA_TYPE_BOOL; + ret_val = allocator.zero_allocate(1U, sizeof(bool), allocator.state); + if (NULL == ret_val) { + return NULL; + } + *((bool *)ret_val) = true; + return ret_val; } - *((bool *)ret_val) = true; - return ret_val; - } - if ((0 == strncmp(value, "N", strlen(value))) || - (0 == strncmp(value, "n", strlen(value))) || - (0 == strncmp(value, "no", strlen(value))) || - (0 == strncmp(value, "No", strlen(value))) || - (0 == strncmp(value, "NO", strlen(value))) || - (0 == strncmp(value, "false", strlen(value))) || - (0 == strncmp(value, "False", strlen(value))) || - (0 == strncmp(value, "FALSE", strlen(value))) || - (0 == strncmp(value, "off", strlen(value))) || - (0 == strncmp(value, "Off", strlen(value))) || - (0 == strncmp(value, "OFF", strlen(value)))) - { - *val_type = DATA_TYPE_BOOL; - ret_val = allocator.zero_allocate(1U, sizeof(bool), allocator.state); - if (NULL == ret_val) { - return NULL; + if ((0 == strcmp(value, "N")) || + (0 == strcmp(value, "n")) || + (0 == strcmp(value, "no")) || + (0 == strcmp(value, "No")) || + (0 == strcmp(value, "NO")) || + (0 == strcmp(value, "false")) || + (0 == strcmp(value, "False")) || + (0 == strcmp(value, "FALSE")) || + (0 == strcmp(value, "off")) || + (0 == strcmp(value, "Off")) || + (0 == strcmp(value, "OFF"))) + { + *val_type = DATA_TYPE_BOOL; + ret_val = allocator.zero_allocate(1U, sizeof(bool), allocator.state); + if (NULL == ret_val) { + return NULL; + } + *((bool *)ret_val) = false; + return ret_val; } - *((bool *)ret_val) = false; - return ret_val; } /// Check for int @@ -834,6 +840,7 @@ static rcl_ret_t parse_value( const size_t parameter_idx = ((params_st->params[node_idx].num_params) - 1U); const size_t val_size = event.data.scalar.length; const char * value = (char *)event.data.scalar.value; + yaml_scalar_style_t style = event.data.scalar.style; const uint32_t line_num = ((uint32_t)(event.start_mark.line) + 1U); rcl_variant_t * param_value; @@ -860,7 +867,7 @@ static rcl_ret_t parse_value( param_value = &(params_st->params[node_idx].parameter_values[parameter_idx]); // param_value->string_value = rcutils_strdup(value, allocator); - ret_val = get_value(value, &val_type, allocator); + ret_val = get_value(value, style, &val_type, allocator); if (NULL == ret_val) { RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("Error parsing value %s at line %d", value, line_num); return RCL_RET_ERROR;