From 83bfd82e653a12ed4d25ccdc437a945419d89f12 Mon Sep 17 00:00:00 2001 From: Andrea Sorbini Date: Mon, 29 Mar 2021 13:27:36 -0700 Subject: [PATCH] Create branch `eloquent` to support Eloquent (#18) * Remove all feature flags and support Eloquent only Signed-off-by: Andrea Sorbini --- README.md | 20 +- rmw_connextdds/CMakeLists.txt | 8 +- rmw_connextdds/src/rmw_api_impl_ndds.cpp | 158 +- rmw_connextdds_common/CMakeLists.txt | 81 +- .../include/rmw_connextdds/context.hpp | 29 +- .../include/rmw_connextdds/context_common.hpp | 4 - .../include/rmw_connextdds/dds_api.hpp | 24 +- .../include/rmw_connextdds/graph_cache.hpp | 5 +- .../include/rmw_connextdds/rmw_api_impl.hpp | 95 +- .../include/rmw_connextdds/rmw_impl.hpp | 82 +- .../rmw_connextdds/rmw_waitset_dds.hpp | 27 - .../rmw_connextdds/rmw_waitset_std.hpp | 14 - .../include/rmw_connextdds/static_config.hpp | 106 -- .../rmw_connextdds/topic_endpoint_info.h | 225 --- .../topic_endpoint_info_array.h | 121 -- rmw_connextdds_common/package.xml | 2 - .../rmw_connextdds_common-extras.cmake.in | 27 - .../src/common/rmw_context.cpp | 284 +-- .../src/common/rmw_graph.cpp | 43 +- rmw_connextdds_common/src/common/rmw_impl.cpp | 243 +-- .../src/common/rmw_impl_waitset_dds.cpp | 8 - .../src/common/rmw_impl_waitset_std.cpp | 16 - rmw_connextdds_common/src/common/rmw_node.cpp | 42 +- .../src/common/rmw_publication.cpp | 22 +- rmw_connextdds_common/src/common/rmw_qos.cpp | 32 - .../src/common/rmw_security_log.cpp | 240 --- .../src/common/rmw_serde.cpp | 10 - .../src/common/rmw_service.cpp | 16 - .../src/common/rmw_subscription.cpp | 67 +- .../src/ndds/dds_api_ndds.cpp | 125 +- .../src/rmw_dds_common/topic_endpoint_info.c | 228 --- .../topic_endpoint_info_array.c | 110 -- .../src/rtime/dds_api_rtime.cpp | 85 +- rmw_connextddsmicro/CMakeLists.txt | 8 +- .../src/rmw_api_impl_rtime.cpp | 156 +- rmw_dds_common/.github/ISSUE_TEMPLATE.md | 45 + rmw_dds_common/CONTRIBUTING.md | 18 + rmw_dds_common/LICENSE | 202 ++ rmw_dds_common/README.md | 17 + rmw_dds_common/rmw_dds_common/CHANGELOG.rst | 36 + rmw_dds_common/rmw_dds_common/CMakeLists.txt | 99 + rmw_dds_common/rmw_dds_common/Doxyfile | 30 + .../rmw_dds_common/QUALITY_DECLARATION.md | 208 +++ .../rmw_dds_common/docs/FEATURES.md | 13 + .../include/rmw_dds_common/context.hpp | 45 + .../include/rmw_dds_common}/gid_utils.hpp | 36 +- .../include/rmw_dds_common/graph_cache.hpp | 132 +- .../rmw_dds_common/topic_endpoint_info.h | 345 ++++ .../topic_endpoint_info_array.h | 155 ++ .../rmw_dds_common/visibility_control.h | 58 + rmw_dds_common/rmw_dds_common/msg/Gid.msg | 1 + .../rmw_dds_common/msg/NodeEntitiesInfo.msg | 4 + .../msg/ParticipantEntitiesInfo.msg | 2 + rmw_dds_common/rmw_dds_common/package.xml | 32 + .../rmw_dds_common/src}/gid_utils.cpp | 10 +- .../rmw_dds_common/src}/graph_cache.cpp | 24 +- .../test/allocator_testing_utils.h | 94 + .../test/benchmark/benchmark_graph_cache.cpp | 405 ++++ .../rmw_dds_common/test/test_gid_utils.cpp | 38 + .../rmw_dds_common/test/test_graph_cache.cpp | 1651 +++++++++++++++++ 60 files changed, 3686 insertions(+), 2777 deletions(-) delete mode 100644 rmw_connextdds_common/include/rmw_connextdds/topic_endpoint_info.h delete mode 100644 rmw_connextdds_common/include/rmw_connextdds/topic_endpoint_info_array.h delete mode 100644 rmw_connextdds_common/rmw_connextdds_common-extras.cmake.in delete mode 100644 rmw_connextdds_common/src/common/rmw_qos.cpp delete mode 100644 rmw_connextdds_common/src/common/rmw_security_log.cpp delete mode 100644 rmw_connextdds_common/src/rmw_dds_common/topic_endpoint_info.c delete mode 100644 rmw_connextdds_common/src/rmw_dds_common/topic_endpoint_info_array.c create mode 100644 rmw_dds_common/.github/ISSUE_TEMPLATE.md create mode 100644 rmw_dds_common/CONTRIBUTING.md create mode 100644 rmw_dds_common/LICENSE create mode 100644 rmw_dds_common/README.md create mode 100644 rmw_dds_common/rmw_dds_common/CHANGELOG.rst create mode 100644 rmw_dds_common/rmw_dds_common/CMakeLists.txt create mode 100644 rmw_dds_common/rmw_dds_common/Doxyfile create mode 100644 rmw_dds_common/rmw_dds_common/QUALITY_DECLARATION.md create mode 100644 rmw_dds_common/rmw_dds_common/docs/FEATURES.md create mode 100644 rmw_dds_common/rmw_dds_common/include/rmw_dds_common/context.hpp rename {rmw_connextdds_common/include/rmw_connextdds => rmw_dds_common/rmw_dds_common/include/rmw_dds_common}/gid_utils.hpp (63%) rename rmw_connextdds_common/include/rmw_connextdds/graph_cache_common.hpp => rmw_dds_common/rmw_dds_common/include/rmw_dds_common/graph_cache.hpp (85%) create mode 100644 rmw_dds_common/rmw_dds_common/include/rmw_dds_common/topic_endpoint_info.h create mode 100644 rmw_dds_common/rmw_dds_common/include/rmw_dds_common/topic_endpoint_info_array.h create mode 100644 rmw_dds_common/rmw_dds_common/include/rmw_dds_common/visibility_control.h create mode 100644 rmw_dds_common/rmw_dds_common/msg/Gid.msg create mode 100644 rmw_dds_common/rmw_dds_common/msg/NodeEntitiesInfo.msg create mode 100644 rmw_dds_common/rmw_dds_common/msg/ParticipantEntitiesInfo.msg create mode 100644 rmw_dds_common/rmw_dds_common/package.xml rename {rmw_connextdds_common/src/rmw_dds_common => rmw_dds_common/rmw_dds_common/src}/gid_utils.cpp (89%) rename {rmw_connextdds_common/src/rmw_dds_common => rmw_dds_common/rmw_dds_common/src}/graph_cache.cpp (97%) create mode 100644 rmw_dds_common/rmw_dds_common/test/allocator_testing_utils.h create mode 100644 rmw_dds_common/rmw_dds_common/test/benchmark/benchmark_graph_cache.cpp create mode 100644 rmw_dds_common/rmw_dds_common/test/test_gid_utils.cpp create mode 100644 rmw_dds_common/rmw_dds_common/test/test_graph_cache.cpp diff --git a/README.md b/README.md index 743598a5..a08c49ae 100644 --- a/README.md +++ b/README.md @@ -81,12 +81,20 @@ The following table summarizes which branch of the repository should be checked out in order to compile the RMW implementations for a specific ROS 2 release: -|ROS 2 Release|Branch| -|-------------|------| -|Rolling |`master`| -|Foxy |`master`| -|Eloquent |`dashing`| -|Dashing |`dashing`| +|ROS 2 Release|Branch|Status| +|-------------|------|------| +|Rolling |`master`|Developed| +|Foxy |`foxy`|LTS (May 2023)| +|Eloquent |`eloquent`|EOL (Nov 2020)| +|Dashing |`dashing`|LTS (May 2021)| + +Branch `master` is actively developed and maintained. It is used to create +other branches for specific ROS 2 releases (starting from Galactic). + +Branches marked as `LTS` will receive updates for critical bug fixes and +important patches only (until they reach `EOL`). + +Branches marked as `EOL` will not receive any future updates. ## RTI Connext DDS Requirements diff --git a/rmw_connextdds/CMakeLists.txt b/rmw_connextdds/CMakeLists.txt index 139bab02..5d0bf2bb 100644 --- a/rmw_connextdds/CMakeLists.txt +++ b/rmw_connextdds/CMakeLists.txt @@ -47,13 +47,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE target_link_libraries(${PROJECT_NAME} rmw_connextdds_common::rmw_connextdds_common_pro) -# Unless we need to link the rmw_dds_common shim library generated -# by rmw_connextdds_common (produced if RMW_CONNEXT_PROVIDE_RMW_DDS_COMMON) -# we don't need to call ament_target_dependencies() since we are already -# linking library rmw_connextdds_common_pro above. -if(RMW_CONNEXT_PROVIDE_RMW_DDS_COMMON) - ament_target_dependencies(${PROJECT_NAME} rmw_connextdds_common) -endif() +ament_target_dependencies(${PROJECT_NAME} rmw_connextdds_common) configure_rmw_library(${PROJECT_NAME}) diff --git a/rmw_connextdds/src/rmw_api_impl_ndds.cpp b/rmw_connextdds/src/rmw_api_impl_ndds.cpp index e080a123..1398544d 100644 --- a/rmw_connextdds/src/rmw_api_impl_ndds.cpp +++ b/rmw_connextdds/src/rmw_api_impl_ndds.cpp @@ -287,40 +287,13 @@ rmw_node_t * rmw_create_node( rmw_context_t * context, const char * name, - const char * ns -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_DASHING - , - size_t domain_id, - const rmw_node_security_options_t * security_options -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - , + const char * ns, size_t domain_id, const rmw_node_security_options_t * security_options, - bool localhost_only -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_FOXY - , - size_t domain_id, - bool localhost_only -#endif /* RMW_CONNEXT_RELEASE */ -) + bool localhost_only) { return rmw_api_connextdds_create_node( - context, name, ns -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_DASHING - , - domain_id, - security_options -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - , - domain_id, - security_options, - localhost_only -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_FOXY - , - domain_id, - localhost_only -#endif /* RMW_CONNEXT_RELEASE */ - ); + context, name, ns, domain_id, security_options, localhost_only); } @@ -337,14 +310,11 @@ rmw_node_get_graph_guard_condition(const rmw_node_t * rmw_node) return rmw_api_connextdds_node_get_graph_guard_condition(rmw_node); } -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - rmw_ret_t rmw_node_assert_liveliness(const rmw_node_t * node) { return rmw_api_connextdds_node_assert_liveliness(node); } -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ /***************************************************************************** * Publication API @@ -385,21 +355,11 @@ rmw_publish_loaned_message( rmw_ret_t rmw_init_publisher_allocation( const rosidl_message_type_support_t * type_support, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT const rosidl_message_bounds_t * message_bounds, -#else - const rosidl_runtime_c__Sequence__bound * message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ rmw_publisher_allocation_t * allocation) { return rmw_api_connextdds_init_publisher_allocation( - type_support, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - message_bounds, -#else - message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ - allocation); + type_support, message_bounds, allocation); } @@ -416,19 +376,11 @@ rmw_create_publisher( const rmw_node_t * node, const rosidl_message_type_support_t * type_supports, const char * topic_name, - const rmw_qos_profile_t * qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , - const rmw_publisher_options_t * publisher_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -) + const rmw_qos_profile_t * qos_policies, + const rmw_publisher_options_t * publisher_options) { return rmw_api_connextdds_create_publisher( - node, type_supports, topic_name, qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , publisher_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - ); + node, type_supports, topic_name, qos_policies, publisher_options); } @@ -518,21 +470,11 @@ rmw_destroy_publisher( rmw_ret_t rmw_get_serialized_message_size( const rosidl_message_type_support_t * type_supports, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT const rosidl_message_bounds_t * message_bounds, -#else - const rosidl_runtime_c__Sequence__bound * message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ size_t * size) { return rmw_api_connextdds_get_serialized_message_size( - type_supports, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - message_bounds, -#else - message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ - size); + type_supports, message_bounds, size); } @@ -564,21 +506,13 @@ rmw_deserialize( rmw_ret_t rmw_take_response( const rmw_client_t * client, -#if RMW_CONNEXT_HAVE_SERVICE_INFO - rmw_service_info_t * request_header, -#else rmw_request_id_t * request_header, -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ void * ros_response, bool * taken) { return rmw_api_connextdds_take_response( client, -#if RMW_CONNEXT_HAVE_SERVICE_INFO - request_header, -#else request_header, -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ ros_response, taken); } @@ -587,21 +521,13 @@ rmw_take_response( rmw_ret_t rmw_take_request( const rmw_service_t * service, -#if RMW_CONNEXT_HAVE_SERVICE_INFO - rmw_service_info_t * request_header, -#else rmw_request_id_t * request_header, -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ void * ros_request, bool * taken) { return rmw_api_connextdds_take_request( service, -#if RMW_CONNEXT_HAVE_SERVICE_INFO request_header, -#else - request_header, -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ ros_request, taken); } @@ -674,21 +600,11 @@ rmw_destroy_service( rmw_ret_t rmw_init_subscription_allocation( const rosidl_message_type_support_t * type_support, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT const rosidl_message_bounds_t * message_bounds, -#else - const rosidl_runtime_c__Sequence__bound * message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ rmw_subscription_allocation_t * allocation) { return rmw_api_connextdds_init_subscription_allocation( - type_support, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - message_bounds, -#else - message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ - allocation); + type_support, message_bounds, allocation); } @@ -706,21 +622,10 @@ rmw_create_subscription( const rosidl_message_type_support_t * type_supports, const char * topic_name, const rmw_qos_profile_t * qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - const rmw_subscription_options_t * subscription_options -#else - bool ignore_local_publications -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -) + const rmw_subscription_options_t * subscription_options) { return rmw_api_connextdds_create_subscription( - node, type_supports, topic_name, qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - subscription_options -#else - ignore_local_publications -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - ); + node, type_supports, topic_name, qos_policies, subscription_options); } @@ -775,25 +680,6 @@ rmw_take_with_info( subscription, ros_message, taken, message_info, allocation); } -#if RMW_CONNEXT_HAVE_TAKE_SEQ - -rmw_ret_t -rmw_take_sequence( - const rmw_subscription_t * subscription, - size_t count, - rmw_message_sequence_t * message_sequence, - rmw_message_info_sequence_t * message_info_sequence, - size_t * taken, - rmw_subscription_allocation_t * allocation) -{ - return rmw_api_connextdds_take_sequence( - subscription, count, message_sequence, message_info_sequence, - taken, allocation); -} - -#endif /* RMW_CONNEXT_HAVE_TAKE_SEQ */ - - rmw_ret_t rmw_take_serialized_message( const rmw_subscription_t * subscription, @@ -910,25 +796,3 @@ rmw_wait( return rmw_api_connextdds_wait( subs, gcs, srvs, cls, evs, wait_set, wait_timeout); } - - -/****************************************************************************** - * QoS Profile functions - ******************************************************************************/ -#if RMW_CONNEXT_HAVE_QOS_PROFILE_API -rmw_ret_t -rmw_qos_profile_check_compatible( - const rmw_qos_profile_t publisher_profile, - const rmw_qos_profile_t subscription_profile, - rmw_qos_compatibility_type_t * compatibility, - char * reason, - size_t reason_size) -{ - return rmw_api_connextdds_qos_profile_check_compatible( - publisher_profile, - subscription_profile, - compatibility, - reason, - reason_size); -} -#endif /* RMW_CONNEXT_HAVE_QOS_PROFILE_API */ diff --git a/rmw_connextdds_common/CMakeLists.txt b/rmw_connextdds_common/CMakeLists.txt index 762db54c..9a60ef62 100644 --- a/rmw_connextdds_common/CMakeLists.txt +++ b/rmw_connextdds_common/CMakeLists.txt @@ -71,9 +71,6 @@ function(rtirmw_add_library) ${_rti_build_DEFINES} ) - target_compile_definitions(${_rti_build_NAME} - PUBLIC RMW_CONNEXT_RELEASE=RMW_CONNEXT_RELEASE_${RMW_CONNEXT_RELEASE}) - set(private_defines) if(NOT "${RMW_CONNEXT_LOG_MODE}" STREQUAL "") @@ -117,7 +114,7 @@ set(RMW_CONNEXT_DEPS rcpputils rmw fastcdr - rosidl_default_runtime + rmw_dds_common rosidl_typesupport_fastrtps_c rosidl_typesupport_fastrtps_cpp rosidl_typesupport_introspection_c @@ -128,32 +125,6 @@ foreach(pkg_dep ${RMW_CONNEXT_DEPS}) find_package(${pkg_dep} REQUIRED) endforeach() -################################################################################ -# Detect target rmw version -################################################################################ -if(NOT DEFINED RMW_CONNEXT_RELEASE) - if(${rmw_VERSION} VERSION_GREATER_EQUAL "2.1.0") - set(RMW_CONNEXT_RELEASE rolling) - elseif(${rmw_VERSION} VERSION_GREATER_EQUAL "0.8.2") - set(RMW_CONNEXT_RELEASE foxy) - elseif(${rmw_VERSION} VERSION_GREATER_EQUAL "0.8.1") - set(RMW_CONNEXT_RELEASE eloquent) - else() - set(RMW_CONNEXT_RELEASE dashing) - endif() -endif() - -string(TOUPPER "${RMW_CONNEXT_RELEASE}" RMW_CONNEXT_RELEASE) -set(RMW_CONNEXT_RELEASE "${RMW_CONNEXT_RELEASE}" CACHE INTERNAL "" FORCE) - -message(STATUS "-- Target ROS Release: ${RMW_CONNEXT_RELEASE}") - -set(RMW_CONNEXT_PROVIDE_RMW_DDS_COMMON false) -if("${RMW_CONNEXT_RELEASE}" STREQUAL "DASHING" - OR "${RMW_CONNEXT_RELEASE}" STREQUAL "ELOQUENT") - set(RMW_CONNEXT_PROVIDE_RMW_DDS_COMMON true) -endif() - ################################################################################ # Common Source Configuration ################################################################################ @@ -170,8 +141,6 @@ set(RMW_CONNEXT_COMMON_SOURCE_CPP src/common/rmw_info.cpp src/common/rmw_node.cpp src/common/rmw_publication.cpp - src/common/rmw_qos.cpp - src/common/rmw_security_log.cpp src/common/rmw_serde.cpp src/common/rmw_service.cpp src/common/rmw_subscription.cpp @@ -197,19 +166,6 @@ set(RMW_CONNEXT_COMMON_SOURCE_HPP include/rmw_connextdds/type_support.hpp include/rmw_connextdds/visibility_control.h) -if(RMW_CONNEXT_PROVIDE_RMW_DDS_COMMON) - list(APPEND RMW_CONNEXT_COMMON_SOURCE_CPP - src/rmw_dds_common/graph_cache.cpp - src/rmw_dds_common/topic_endpoint_info.c - src/rmw_dds_common/topic_endpoint_info_array.c - src/rmw_dds_common/gid_utils.cpp) - list(APPEND RMW_CONNEXT_COMMON_SOURCE_HPP - include/rmw_connextdds/graph_cache_common.hpp - include/rmw_connextdds/topic_endpoint_info.h - include/rmw_connextdds/topic_endpoint_info_array.h - include/rmw_connextdds/gid_utils.hpp) -endif() - set(RMW_CONNEXT_COMMON_SOURCE ${RMW_CONNEXT_COMMON_SOURCE_CPP} ${RMW_CONNEXT_COMMON_SOURCE_HPP}) @@ -329,37 +285,6 @@ if(TARGET ${PROJECT_NAME}_pro OR TARGET ${PROJECT_NAME}_micro) set(RMW_CONNEXT_BUILT true) endif() -if(RMW_CONNEXT_BUILT AND RMW_CONNEXT_PROVIDE_RMW_DDS_COMMON) - # Set STATIC_${type_support_type_uppercase} variables to explicitly request - # the generation of the required type supports (fastrtps, and introspection). - set(STATIC_ROSIDL_TYPESUPPORT_C rosidl_typesupport_fastrtps_c - rosidl_typesupport_introspection_c) - set(STATIC_ROSIDL_TYPESUPPORT_CPP rosidl_typesupport_fastrtps_cpp - rosidl_typesupport_introspection_cpp) - - rosidl_generate_interfaces( - ${PROJECT_NAME} - "msg/Gid.msg" - "msg/NodeEntitiesInfo.msg" - "msg/ParticipantEntitiesInfo.msg" - ) - - if(TARGET ${PROJECT_NAME}_pro) - add_dependencies(${PROJECT_NAME}_pro ${PROJECT_NAME}) - target_include_directories(${PROJECT_NAME}_pro - PUBLIC - "$" - "$") - endif() - if(TARGET ${PROJECT_NAME}_micro) - add_dependencies(${PROJECT_NAME}_micro ${PROJECT_NAME}) - target_include_directories(${PROJECT_NAME}_micro - PUBLIC - "$" - "$") - endif() -endif() - ################################################################################ # Configure Ament package ################################################################################ @@ -379,6 +304,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_package( - CONFIG_EXTRAS_POST "${PROJECT_NAME}-extras.cmake.in" -) +ament_package() diff --git a/rmw_connextdds_common/include/rmw_connextdds/context.hpp b/rmw_connextdds_common/include/rmw_connextdds/context.hpp index 8f8efa2e..0abbb286 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/context.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/context.hpp @@ -33,33 +33,14 @@ #include "rmw/get_node_info_and_types.h" #include "rmw/get_service_names_and_types.h" #include "rmw/get_topic_names_and_types.h" -#if RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON -#include "rmw/topic_endpoint_info_array.h" -#include "rmw/get_topic_endpoint_info.h" -#else -#include "rmw_connextdds/topic_endpoint_info_array.h" -#endif /* RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON */ - -#if RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON +#include "rmw_dds_common/topic_endpoint_info_array.h" + #include "rmw_dds_common/context.hpp" #include "rmw_dds_common/msg/participant_entities_info.hpp" -#else -#include "rmw_connextdds/context_common.hpp" -#include "rmw_connextdds_common/msg/participant_entities_info.hpp" -#endif /* RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON */ - -#if RMW_CONNEXT_HAVE_QOS_PROFILE_API -#include "rmw/qos_profiles.h" -#include "rmw_dds_common/qos.hpp" -#endif /* RMW_CONNEXT_HAVE_QOS_PROFILE_API */ #include "rcutils/strdup.h" -#if RMW_CONNEXT_HAVE_SCOPE_EXIT -#include "rcpputils/scope_exit.hpp" -#else -#include "scope_exit.hpp" -#endif /* RMW_CONNEXT_HAVE_SCOPE_EXIT */ +#include "rmw_connextdds/scope_exit.hpp" extern DDS_DomainParticipantFactory * RMW_Connext_gv_DomainParticipantFactory; extern size_t RMW_Connext_gv_ContextCount; @@ -150,11 +131,7 @@ struct rmw_context_impl_t dr_participants(nullptr), dr_publications(nullptr), dr_subscriptions(nullptr), -#if RMW_CONNEXT_HAVE_LOCALHOST_ONLY - localhost_only(base->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) -#else localhost_only(false) -#endif /* RMW_CONNEXT_HAVE_LOCALHOST_ONLY */ { /* destructor relies on these being initialized properly */ common.thread_is_running.store(false); diff --git a/rmw_connextdds_common/include/rmw_connextdds/context_common.hpp b/rmw_connextdds_common/include/rmw_connextdds/context_common.hpp index 25e25d1b..126e5de5 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/context_common.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/context_common.hpp @@ -17,8 +17,6 @@ #include "rmw_connextdds/static_config.hpp" -#if !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON - #include #include #include @@ -57,6 +55,4 @@ struct Context } // namespace rmw_dds_common -#endif /* !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON */ - #endif // RMW_CONNEXTDDS__CONTEXT_COMMON_HPP_ diff --git a/rmw_connextdds_common/include/rmw_connextdds/dds_api.hpp b/rmw_connextdds_common/include/rmw_connextdds/dds_api.hpp index e07fe4a0..f342e4b3 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/dds_api.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/dds_api.hpp @@ -86,12 +86,8 @@ rmw_connextdds_get_datawriter_qos( RMW_Connext_MessageTypeSupport * const type_support, DDS_Topic * const topic, DDS_DataWriterQos * const qos, - const rmw_qos_profile_t * const qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , - const rmw_publisher_options_t * const pub_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -); + const rmw_qos_profile_t * const qos_policies, + const rmw_publisher_options_t * const pub_options); rmw_ret_t rmw_connextdds_get_datareader_qos( @@ -99,36 +95,28 @@ rmw_connextdds_get_datareader_qos( RMW_Connext_MessageTypeSupport * const type_support, DDS_TopicDescription * const topic_desc, DDS_DataReaderQos * const qos, - const rmw_qos_profile_t * const qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , - const rmw_subscription_options_t * const sub_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -); + const rmw_qos_profile_t * const qos_policies, + const rmw_subscription_options_t * const sub_options); DDS_DataWriter * - rmw_connextdds_create_datawriter( +rmw_connextdds_create_datawriter( rmw_context_impl_t * const ctx, DDS_DomainParticipant * const participant, DDS_Publisher * const pub, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_publisher_options_t * const publisher_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal, RMW_Connext_MessageTypeSupport * const type_support, DDS_Topic * const topic, DDS_DataWriterQos * const dw_qos); DDS_DataReader * - rmw_connextdds_create_datareader( +rmw_connextdds_create_datareader( rmw_context_impl_t * const ctx, DDS_DomainParticipant * const participant, DDS_Subscriber * const sub, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_subscription_options_t * const subscriber_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal, RMW_Connext_MessageTypeSupport * const type_support, DDS_TopicDescription * const topic_desc, diff --git a/rmw_connextdds_common/include/rmw_connextdds/graph_cache.hpp b/rmw_connextdds_common/include/rmw_connextdds/graph_cache.hpp index fafd5113..570891e2 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/graph_cache.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/graph_cache.hpp @@ -105,7 +105,7 @@ rmw_connextdds_graph_remove_participant( const DDS_InstanceHandle_t * const instance); rmw_ret_t - rmw_connextdds_graph_add_remote_entity( +rmw_connextdds_graph_add_remote_entity( rmw_context_impl_t * ctx, const DDS_GUID_t * const endp_guid, const DDS_GUID_t * const dp_guid, @@ -115,9 +115,6 @@ rmw_ret_t const DDS_DurabilityQosPolicy * const durability, const DDS_DeadlineQosPolicy * const deadline, const DDS_LivelinessQosPolicy * const liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - const DDS_LifespanQosPolicy * const lifespan, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ const bool is_reader); rmw_ret_t diff --git a/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp b/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp index 7a2a03aa..59eed8d0 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/rmw_api_impl.hpp @@ -204,22 +204,10 @@ rmw_node_t * rmw_api_connextdds_create_node( rmw_context_t * context, const char * name, - const char * ns -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_DASHING - , - size_t domain_id, - const rmw_node_security_options_t * security_options -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - , + const char * ns, size_t domain_id, const rmw_node_security_options_t * security_options, - bool localhost_only -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_FOXY - , - size_t domain_id, - bool localhost_only -#endif /* RMW_CONNEXT_RELEASE */ -); + bool localhost_only); RMW_CONNEXTDDS_PUBLIC @@ -230,11 +218,9 @@ RMW_CONNEXTDDS_PUBLIC const rmw_guard_condition_t * rmw_api_connextdds_node_get_graph_guard_condition(const rmw_node_t * rmw_node); -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT RMW_CONNEXTDDS_PUBLIC rmw_ret_t rmw_api_connextdds_node_assert_liveliness(const rmw_node_t * node); -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ /***************************************************************************** * Publication API @@ -262,13 +248,9 @@ rmw_api_connextdds_publish_loaned_message( RMW_CONNEXTDDS_PUBLIC rmw_ret_t - rmw_api_connextdds_init_publisher_allocation( +rmw_api_connextdds_init_publisher_allocation( const rosidl_message_type_support_t * type_support, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT const rosidl_message_bounds_t * message_bounds, -#else - const rosidl_runtime_c__Sequence__bound * message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ rmw_publisher_allocation_t * allocation); RMW_CONNEXTDDS_PUBLIC @@ -282,12 +264,8 @@ rmw_api_connextdds_create_publisher( const rmw_node_t * node, const rosidl_message_type_support_t * type_supports, const char * topic_name, - const rmw_qos_profile_t * qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , - const rmw_publisher_options_t * publisher_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -); + const rmw_qos_profile_t * qos_policies, + const rmw_publisher_options_t * publisher_options); RMW_CONNEXTDDS_PUBLIC rmw_ret_t @@ -343,13 +321,9 @@ rmw_api_connextdds_destroy_publisher( *****************************************************************************/ RMW_CONNEXTDDS_PUBLIC rmw_ret_t - rmw_api_connextdds_get_serialized_message_size( +rmw_api_connextdds_get_serialized_message_size( const rosidl_message_type_support_t * type_supports, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT const rosidl_message_bounds_t * message_bounds, -#else - const rosidl_runtime_c__Sequence__bound * message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ size_t * size); RMW_CONNEXTDDS_PUBLIC @@ -371,25 +345,17 @@ rmw_api_connextdds_deserialize( *****************************************************************************/ RMW_CONNEXTDDS_PUBLIC rmw_ret_t - rmw_api_connextdds_take_response( +rmw_api_connextdds_take_response( const rmw_client_t * client, -#if RMW_CONNEXT_HAVE_SERVICE_INFO - rmw_service_info_t * request_header, -#else rmw_request_id_t * request_header, -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ void * ros_response, bool * taken); RMW_CONNEXTDDS_PUBLIC rmw_ret_t - rmw_api_connextdds_take_request( +rmw_api_connextdds_take_request( const rmw_service_t * service, -#if RMW_CONNEXT_HAVE_SERVICE_INFO - rmw_service_info_t * request_header, -#else rmw_request_id_t * request_header, -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ void * ros_request, bool * taken); @@ -439,13 +405,9 @@ rmw_api_connextdds_destroy_service( *****************************************************************************/ RMW_CONNEXTDDS_PUBLIC rmw_ret_t - rmw_api_connextdds_init_subscription_allocation( +rmw_api_connextdds_init_subscription_allocation( const rosidl_message_type_support_t * type_support, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT const rosidl_message_bounds_t * message_bounds, -#else - const rosidl_runtime_c__Sequence__bound * message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ rmw_subscription_allocation_t * allocation); RMW_CONNEXTDDS_PUBLIC @@ -455,17 +417,12 @@ rmw_api_connextdds_fini_subscription_allocation( RMW_CONNEXTDDS_PUBLIC rmw_subscription_t * - rmw_api_connextdds_create_subscription( +rmw_api_connextdds_create_subscription( const rmw_node_t * node, const rosidl_message_type_support_t * type_supports, const char * topic_name, const rmw_qos_profile_t * qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - const rmw_subscription_options_t * subscription_options -#else - bool ignore_local_publications -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - ); + const rmw_subscription_options_t * subscription_options); RMW_CONNEXTDDS_PUBLIC @@ -504,21 +461,6 @@ rmw_api_connextdds_take_with_info( rmw_message_info_t * message_info, rmw_subscription_allocation_t * allocation); -#if RMW_CONNEXT_HAVE_TAKE_SEQ - -RMW_CONNEXTDDS_PUBLIC -rmw_ret_t -rmw_api_connextdds_take_sequence( - const rmw_subscription_t * subscription, - size_t count, - rmw_message_sequence_t * message_sequence, - rmw_message_info_sequence_t * message_info_sequence, - size_t * taken, - rmw_subscription_allocation_t * allocation); - -#endif /* RMW_CONNEXT_HAVE_TAKE_SEQ */ - - RMW_CONNEXTDDS_PUBLIC rmw_ret_t rmw_api_connextdds_take_serialized_message( @@ -604,19 +546,4 @@ rmw_api_connextdds_wait( rmw_wait_set_t * wait_set, const rmw_time_t * wait_timeout); -/****************************************************************************** - * QoS Profile functions - ******************************************************************************/ -#if RMW_CONNEXT_HAVE_QOS_PROFILE_API -RMW_CONNEXTDDS_PUBLIC -rmw_ret_t -rmw_api_connextdds_qos_profile_check_compatible( - const rmw_qos_profile_t publisher_profile, - const rmw_qos_profile_t subscription_profile, - rmw_qos_compatibility_type_t * compatibility, - char * reason, - size_t reason_size); -#endif /* RMW_CONNEXT_HAVE_QOS_PROFILE_API */ - - #endif // RMW_CONNEXTDDS__RMW_API_IMPL_HPP_ diff --git a/rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp b/rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp index c3ceca5e..d4a425fd 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/rmw_impl.hpp @@ -31,8 +31,6 @@ #include "rcutils/types/uint8_array.h" #include "rcpputils/thread_safety_annotations.hpp" -#if !RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS - /****************************************************************************** * Symbols provided by rmw/incompatible_qos.h from Foxy onward ******************************************************************************/ @@ -63,35 +61,12 @@ typedef struct rmw_qos_incompatible_event_status_t rmw_requested_qos_incompatibl typedef struct rmw_qos_incompatible_event_status_t rmw_offered_qos_incompatible_event_status_t; -#endif /* !RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS */ - -#if !RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS_EVENT - -/****************************************************************************** - * Define value for RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE, and - * RMW_EVENT_OFFERED_QOS_INCOMPATIBLE as invalid values, - * since they're not defined by rmw_event_type_t. - ******************************************************************************/ -#define RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE (RMW_EVENT_INVALID + 1) -#define RMW_EVENT_OFFERED_QOS_INCOMPATIBLE (RMW_EVENT_INVALID + 2) - -#endif /* !RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS_EVENT */ - -#if !RMW_CONNEXT_HAVE_MESSAGE_LOST -/****************************************************************************** - * Define value for RMW_EVENT_MESSAGE_LOST as an invalid value, - * since it's not defined by rmw_event_type_t. - ******************************************************************************/ -#define RMW_EVENT_MESSAGE_LOST (RMW_EVENT_INVALID + 3) - typedef struct RMW_PUBLIC_TYPE rmw_message_lost_status_t { size_t total_count; size_t total_count_change; } rmw_message_lost_status_t; -#endif /* !RMW_CONNEXT_HAVE_MESSAGE_LOST */ -#if !RMW_CONNEXT_HAVE_SERVICE_INFO typedef rcutils_time_point_value_t rmw_time_point_value_t; typedef struct RMW_PUBLIC_TYPE rmw_service_info_t @@ -100,7 +75,6 @@ typedef struct RMW_PUBLIC_TYPE rmw_service_info_t rmw_time_point_value_t received_timestamp; rmw_request_id_t request_id; } rmw_service_info_t; -#endif /* !RMW_CONNEXT_HAVE_SERVICE_INFO */ /****************************************************************************** * General helpers and utilities. @@ -180,16 +154,14 @@ class RMW_Connext_Publisher { public: static RMW_Connext_Publisher * - create( + create( rmw_context_impl_t * const ctx, DDS_DomainParticipant * const dp, DDS_Publisher * const pub, const rosidl_message_type_support_t * const type_supports, const char * const topic_name, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_publisher_options_t * const publisher_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal = false, const RMW_Connext_MessageType msg_type = RMW_CONNEXT_MESSAGE_USERDATA, const void * const intro_members = nullptr, @@ -313,7 +285,7 @@ class RMW_Connext_Publisher }; rmw_publisher_t * - rmw_connextdds_create_publisher( +rmw_connextdds_create_publisher( rmw_context_impl_t * const ctx, const rmw_node_t * const node, DDS_DomainParticipant * const dp, @@ -321,9 +293,7 @@ rmw_publisher_t * const rosidl_message_type_support_t * const type_supports, const char * const topic_name, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_publisher_options_t * const publisher_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal = false); rmw_ret_t @@ -339,18 +309,14 @@ class RMW_Connext_Subscriber { public: static RMW_Connext_Subscriber * - create( + create( rmw_context_impl_t * const ctx, DDS_DomainParticipant * const dp, DDS_Subscriber * const sub, const rosidl_message_type_support_t * const type_supports, const char * const topic_name, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_subscription_options_t * const subscriber_options, -#else - const bool ignore_local_publications, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal = false, const RMW_Connext_MessageType msg_type = RMW_CONNEXT_MESSAGE_USERDATA, const void * const intro_members = nullptr, @@ -503,15 +469,6 @@ class RMW_Connext_Subscriber bool * const taken, const DDS_InstanceHandle_t * const request_writer_handle = nullptr); -#if RMW_CONNEXT_HAVE_TAKE_SEQ - rmw_ret_t - take( - rmw_message_sequence_t * const message_sequence, - rmw_message_info_sequence_t * const message_info_sequence, - const size_t max_samples, - size_t * const taken); -#endif /* RMW_CONNEXT_HAVE_TAKE_SEQ */ - rmw_ret_t take_serialized( rmw_serialized_message_t * const serialized_message, @@ -580,7 +537,7 @@ class RMW_Connext_Subscriber }; rmw_subscription_t * - rmw_connextdds_create_subscriber( +rmw_connextdds_create_subscriber( rmw_context_impl_t * const ctx, const rmw_node_t * const node, DDS_DomainParticipant * const dp, @@ -588,11 +545,7 @@ rmw_subscription_t * const rosidl_message_type_support_t * const type_supports, const char * const topic_name, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_subscription_options_t * const subscriber_options, -#else - const bool ignore_local_publications, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal = false); rmw_ret_t @@ -843,7 +796,7 @@ rmw_connextdds_create_topic_name( ******************************************************************************/ rmw_ret_t - rmw_connextdds_get_readerwriter_qos( +rmw_connextdds_get_readerwriter_qos( const bool writer_qos, RMW_Connext_MessageTypeSupport * const type_support, DDS_HistoryQosPolicy * const history, @@ -853,36 +806,17 @@ rmw_ret_t DDS_LivelinessQosPolicy * const liveliness, DDS_ResourceLimitsQosPolicy * const resource_limits, DDS_PublishModeQosPolicy * const publish_mode, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - DDS_LifespanQosPolicy * const lifespan, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ - const rmw_qos_profile_t * const qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , + const rmw_qos_profile_t * const qos_policies, const rmw_publisher_options_t * const pub_options, - const rmw_subscription_options_t * const sub_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - ); + const rmw_subscription_options_t * const sub_options); rmw_ret_t - rmw_connextdds_readerwriter_qos_to_ros( +rmw_connextdds_readerwriter_qos_to_ros( const DDS_HistoryQosPolicy * const history, const DDS_ReliabilityQosPolicy * const reliability, const DDS_DurabilityQosPolicy * const durability, const DDS_DeadlineQosPolicy * const deadline, const DDS_LivelinessQosPolicy * const liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - const DDS_LifespanQosPolicy * const lifespan, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ rmw_qos_profile_t * const qos_policies); -/****************************************************************************** - * Security Helpers - ******************************************************************************/ -#if RMW_CONNEXT_HAVE_SECURITY -rmw_ret_t -rmw_connextdds_apply_security_logging_configuration( - DDS_PropertyQosPolicy * const properties); -#endif /* RMW_CONNEXT_HAVE_SECURITY */ - #endif // RMW_CONNEXTDDS__RMW_IMPL_HPP_ diff --git a/rmw_connextdds_common/include/rmw_connextdds/rmw_waitset_dds.hpp b/rmw_connextdds_common/include/rmw_connextdds/rmw_waitset_dds.hpp index ca307400..510b8dd2 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/rmw_waitset_dds.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/rmw_waitset_dds.hpp @@ -297,13 +297,6 @@ class RMW_Connext_GuardCondition : public RMW_Connext_Condition // DomainParticipantFactory has already been finalized. // Don't try to delete the guard condition, or we might // end up with a segfault. -#if RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_FOXY - // For ROS2 releases > Foxy, this is unexpected behavior - // so print an error message. - RMW_CONNEXT_LOG_ERROR( - "DomainParticipantFactory already finalized, " - "leaked DDS guard condition") -#endif /* RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_FOXY */ return; } this->invalidate(); @@ -695,26 +688,6 @@ class RMW_Connext_SubscriberStatusCondition : public RMW_Connext_StatusCondition return RMW_RET_OK; } -#if RMW_CONNEXT_HAVE_MESSAGE_LOST - inline rmw_ret_t - get_message_lost_status(rmw_message_lost_status_t * const status) - { - DDS_SampleLostStatus dds_status = DDS_SampleLostStatus_INITIALIZER; - - if (DDS_RETCODE_OK != - DDS_DataReader_get_sample_lost_status(this->reader, &dds_status)) - { - RMW_CONNEXT_LOG_ERROR_SET("failed to get sample lost status") - return RMW_RET_ERROR; - } - - status->total_count = dds_status.total_count; - status->total_count_change = dds_status.total_count_change; - - return RMW_RET_OK; - } -#endif /* RMW_CONNEXT_HAVE_MESSAGE_LOST */ - protected: rmw_ret_t install(); diff --git a/rmw_connextdds_common/include/rmw_connextdds/rmw_waitset_std.hpp b/rmw_connextdds_common/include/rmw_connextdds/rmw_waitset_std.hpp index 0b1f5b7c..7aa0d811 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/rmw_waitset_std.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/rmw_waitset_std.hpp @@ -661,20 +661,6 @@ class RMW_Connext_SubscriberStatusCondition : public RMW_Connext_StatusCondition return RMW_RET_OK; } -#if RMW_CONNEXT_HAVE_MESSAGE_LOST - inline rmw_ret_t - get_message_lost_status(rmw_message_lost_status_t * const status) - { - status->total_count = this->status_sample_lost.total_count; - status->total_count_change = this->status_sample_lost.total_count_change; - - this->status_sample_lost.total_count_change = 0; - this->triggered_sample_lost = false; - - return RMW_RET_OK; - } -#endif /* RMW_CONNEXT_HAVE_MESSAGE_LOST */ - protected: void update_status_deadline( const DDS_RequestedDeadlineMissedStatus * const status); diff --git a/rmw_connextdds_common/include/rmw_connextdds/static_config.hpp b/rmw_connextdds_common/include/rmw_connextdds/static_config.hpp index 3ed12264..f4732d28 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/static_config.hpp +++ b/rmw_connextdds_common/include/rmw_connextdds/static_config.hpp @@ -306,112 +306,6 @@ #define RMW_CONNEXT_CPP_STD_WAITSETS 0 #endif /* RMW_CONNEXT_CPP_STD_WAITSETS */ -/****************************************************************************** - * ROS Target Release - ******************************************************************************/ -#define RMW_CONNEXT_RELEASE_DASHING 10 -#define RMW_CONNEXT_RELEASE_ELOQUENT 20 -#define RMW_CONNEXT_RELEASE_FOXY 30 -#define RMW_CONNEXT_RELEASE_ROLLING 40 - -#ifndef RMW_CONNEXT_RELEASE -#define RMW_CONNEXT_RELEASE RMW_CONNEXT_RELEASE_DASHING -#endif /* RMW_CONNEXT_RELEASE */ - -#ifndef RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON -#define RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON \ - (RMW_CONNEXT_RELEASE >= RMW_CONNEXT_RELEASE_FOXY) -#endif /* RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON */ - -#ifndef RMW_CONNEXT_HAVE_SCOPE_EXIT -#define RMW_CONNEXT_HAVE_SCOPE_EXIT \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_FOXY) -#endif /* RMW_CONNEXT_HAVE_SCOPE_EXIT */ - -#ifndef RMW_CONNEXT_HAVE_GET_DOMAIN -#define RMW_CONNEXT_HAVE_GET_DOMAIN \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_FOXY) -#endif /* RMW_CONNEXT_HAVE_SCOPE_EXIT */ - -#ifndef RMW_CONNEXT_HAVE_OPTIONS -#define RMW_CONNEXT_HAVE_OPTIONS \ - (RMW_CONNEXT_RELEASE >= RMW_CONNEXT_RELEASE_FOXY) -#endif /* RMW_CONNEXT_HAVE_OPTIONS */ - -#ifndef RMW_CONNEXT_HAVE_OPTIONS_PUBSUB -#define RMW_CONNEXT_HAVE_OPTIONS_PUBSUB \ - (RMW_CONNEXT_RELEASE >= RMW_CONNEXT_RELEASE_ELOQUENT) -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - -#ifndef RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS -#define RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_FOXY) -#endif /* RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS */ - -#ifndef RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS_EVENT -#define RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS_EVENT \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_ELOQUENT) -#endif /* RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS_EVENT */ - -#ifndef RMW_CONNEXT_HAVE_LIFESPAN_QOS -#define RMW_CONNEXT_HAVE_LIFESPAN_QOS \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_FOXY) -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ - -#ifndef RMW_CONNEXT_HAVE_MESSAGE_LOST -#define RMW_CONNEXT_HAVE_MESSAGE_LOST \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_FOXY) -#endif /* RMW_CONNEXT_HAVE_MESSAGE_LOST */ - -#ifndef RMW_CONNEXT_HAVE_SERVICE_INFO -#define RMW_CONNEXT_HAVE_SERVICE_INFO \ - (RMW_CONNEXT_RELEASE >= RMW_CONNEXT_RELEASE_FOXY) -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ - -#ifndef RMW_CONNEXT_HAVE_LOCALHOST_ONLY -#define RMW_CONNEXT_HAVE_LOCALHOST_ONLY \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_ELOQUENT) -#endif /* RMW_CONNEXT_HAVE_LOCALHOST_ONLY */ - -#ifndef RMW_CONNEXT_HAVE_TAKE_SEQ -#define RMW_CONNEXT_HAVE_TAKE_SEQ \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_ELOQUENT) -#endif /* RMW_CONNEXT_HAVE_TAKE_SEQ */ - -#ifndef RMW_CONNEXT_HAVE_SECURITY -#define RMW_CONNEXT_HAVE_SECURITY \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_ELOQUENT) -#endif /* RMW_CONNEXT_HAVE_SECURITY */ - -#ifndef RMW_CONNEXT_HAVE_COMMON_MUTEX -#define RMW_CONNEXT_HAVE_COMMON_MUTEX RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON -#endif /* RMW_CONNEXT_HAVE_COMMON_MUTEX */ - -#ifndef RMW_CONNEXT_HAVE_LOAN_MESSAGE -#define RMW_CONNEXT_HAVE_LOAN_MESSAGE \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_ELOQUENT) -#endif /* RMW_CONNEXT_HAVE_LOAN_MESSAGE */ - -#ifndef RMW_CONNEXT_HAVE_MESSAGE_INFO_TS -#define RMW_CONNEXT_HAVE_MESSAGE_INFO_TS \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_ELOQUENT) -#endif /* RMW_CONNEXT_HAVE_MESSAGE_INFO_TS */ - -#ifndef RMW_CONNEXT_HAVE_GET_INFO_BY_TOPIC -#define RMW_CONNEXT_HAVE_GET_INFO_BY_TOPIC \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_ELOQUENT) -#endif /* RMW_CONNEXT_HAVE_GET_INFO_BY_TOPIC */ - -#ifndef RMW_CONNEXT_HAVE_QOS_PROFILE_API -#define RMW_CONNEXT_HAVE_QOS_PROFILE_API \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_FOXY) -#endif /* RMW_CONNEXT_HAVE_QOS_PROFILE_API */ - -#ifndef RMW_CONNEXT_HAVE_TIME_UTILS -#define RMW_CONNEXT_HAVE_TIME_UTILS \ - (RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_FOXY) -#endif /* RMW_CONNEXT_HAVE_TIME_UTILS */ - #include "resource_limits.hpp" #endif // RMW_CONNEXTDDS__STATIC_CONFIG_HPP_ diff --git a/rmw_connextdds_common/include/rmw_connextdds/topic_endpoint_info.h b/rmw_connextdds_common/include/rmw_connextdds/topic_endpoint_info.h deleted file mode 100644 index 459fd517..00000000 --- a/rmw_connextdds_common/include/rmw_connextdds/topic_endpoint_info.h +++ /dev/null @@ -1,225 +0,0 @@ -// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. -// -// 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 RMW_CONNEXTDDS__TOPIC_ENDPOINT_INFO_H_ -#define RMW_CONNEXTDDS__TOPIC_ENDPOINT_INFO_H_ - -#include "rmw_connextdds/static_config.hpp" - -#if !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "rcutils/allocator.h" -#include "rmw/types.h" -#include "rmw/visibility_control.h" - -/// Endpoint enumeration type -typedef enum RMW_PUBLIC_TYPE rmw_endpoint_type_t -{ - /// Endpoint type has not yet been set - RMW_ENDPOINT_INVALID = 0, - - /// Creates and publishes messages to the ROS topic - RMW_ENDPOINT_PUBLISHER, - - /// Listens for and receives messages from a topic - RMW_ENDPOINT_SUBSCRIPTION -} rmw_endpoint_type_t; - -/// A structure that encapsulates the name, namespace, topic_type, gid and qos_profile -/// of publishers and subscriptions for a topic. -typedef struct RMW_PUBLIC_TYPE rmw_topic_endpoint_info_t -{ - /// Name of the node - const char * node_name; - /// Namespace of the node - const char * node_namespace; - /// The associated topic type - const char * topic_type; - /// The endpoint type - rmw_endpoint_type_t endpoint_type; - /// The GID of the endpoint - uint8_t endpoint_gid[RMW_GID_STORAGE_SIZE]; - /// QoS profile of the endpoint - rmw_qos_profile_t qos_profile; -} rmw_topic_endpoint_info_t; - -/// Return a rmw_topic_endpoint_info_t struct with members initialized to `NULL`. -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_topic_endpoint_info_t -rmw_get_zero_initialized_topic_endpoint_info(void); - -/// Finalize a rmw_topic_endpoint_info_t object. -/** - * The rmw_topic_endpoint_info_t struct has members which require memory to be allocated to them before - * setting values. - * This function reclaims any allocated resources within the object and zeroes out all other - * members. - * - * If a non RMW_RET_OK return value is returned, the RMW error message will be set - * - * \param[inout] topic_endpoint_info object to be finalized - * \param[in] allocator the allocator used to allocate memory to the object - * \returns `RMW_RET_OK` on successfully reclaiming memory, or - * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or - * \returns `RMW_RET_ERROR` when an unspecified error occurs. - */ -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_ret_t -rmw_topic_endpoint_info_fini( - rmw_topic_endpoint_info_t * topic_endpoint_info, - rcutils_allocator_t * allocator); - -/// Set the topic_type in rmw_topic_endpoint_info_t. -/** - * rmw_topic_endpoint_info_t has a member topic_type of type const char *; - * this function allocates memory and copies the value of param passed to it. - * - * If a non RMW_RET_OK return value is returned, the RMW error message will be set - * - * \param[inout] topic_endpoint_info pointer to an initialized instance of rmw_topic_endpoint_info_t - * \param[in] topic_type the topic_type value to set in rmw_topic_endpoint_info_t - * \param[in] allocator the allocator that will be used to allocate memory - * \returns `RMW_RET_OK` on successfully setting the topic_type, or - * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or - * \returns `RMW_RET_ERROR` when an unspecified error occurs. - */ -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_ret_t -rmw_topic_endpoint_info_set_topic_type( - rmw_topic_endpoint_info_t * topic_endpoint_info, - const char * topic_type, - rcutils_allocator_t * allocator); - -/// Set the node_name in rmw_topic_endpoint_info_t. -/** - * rmw_topic_endpoint_info_t has a member node_name of type const char *; - * this function allocates memory and copies the value of param passed to it. - * - * If a non RMW_RET_OK return value is returned, the RMW error message will be set - * - * \param[inout] topic_endpoint_info pointer to an initialized instance of rmw_topic_endpoint_info_t - * \param[in] node_name the node_name value to set in rmw_topic_endpoint_info_t - * \param[in] allocator the allocator that will be used to allocate memory - * \returns `RMW_RET_OK` on successfully setting the node_name, or - * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or - * \returns `RMW_RET_ERROR` when an unspecified error occurs. - */ -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_ret_t -rmw_topic_endpoint_info_set_node_name( - rmw_topic_endpoint_info_t * topic_endpoint_info, - const char * node_name, - rcutils_allocator_t * allocator); - -/// Set the node_namespace in rmw_topic_endpoint_info_t. -/** - * rmw_topic_endpoint_info_t has a member node_namespace of type const char *; - * this function allocates memory and copies the value of param passed to it. - * - * If a non RMW_RET_OK return value is returned, the RMW error message will be set - * - * \param[inout] topic_endpoint_info pointer to an initialized instance of rmw_topic_endpoint_info_t - * \param[in] node_namespace the node_namespace value to set in rmw_topic_endpoint_info_t - * \param[in] allocator the allocator that will be used to allocate memory - * \returns `RMW_RET_OK` on successfully setting the node_namespace, or - * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or - * \returns `RMW_RET_ERROR` when an unspecified error occurs. - */ -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_ret_t -rmw_topic_endpoint_info_set_node_namespace( - rmw_topic_endpoint_info_t * topic_endpoint_info, - const char * node_namespace, - rcutils_allocator_t * allocator); - -/// Set the gid in rmw_topic_endpoint_info_t. -/** - * Copies the values from gid into the gid member inside topic_endpoint_info. - * - * If a non RMW_RET_OK return value is returned, the RMW error message will be set - * - * \param[inout] topic_endpoint_info pointer to an initialized instance of rmw_topic_endpoint_info_t - * \param[in] gid the gid value to set in rmw_topic_endpoint_info_t - * \param[in] size the size of the gid param - * \returns `RMW_RET_OK` on successfully setting the gid, or - * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or - * \returns `RMW_RET_INVALID_ARGUMENT` size is greater than RMW_GID_STORAGE_SIZE, or - * \returns `RMW_RET_ERROR` when an unspecified error occurs. - */ -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_ret_t -rmw_topic_endpoint_info_set_endpoint_type( - rmw_topic_endpoint_info_t * topic_endpoint_info, - rmw_endpoint_type_t type); - -/// Set the gid in rmw_topic_endpoint_info_t. -/** - * Copies the values from gid into the gid member inside topic_endpoint_info. - * - * If a non RMW_RET_OK return value is returned, the RMW error message will be set - * - * \param[inout] topic_endpoint_info pointer to an initialized instance of rmw_topic_endpoint_info_t - * \param[in] gid the gid value to set in rmw_topic_endpoint_info_t - * \param[in] size the size of the gid param - * \returns `RMW_RET_OK` on successfully setting the gid, or - * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or - * \returns `RMW_RET_INVALID_ARGUMENT` size is greater than RMW_GID_STORAGE_SIZE, or - * \returns `RMW_RET_ERROR` when an unspecified error occurs. - */ -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_ret_t -rmw_topic_endpoint_info_set_gid( - rmw_topic_endpoint_info_t * topic_endpoint_info, - const uint8_t gid[], - size_t size); - -/// Set the qos_profile in rmw_topic_endpoint_info_t. -/** - * rmw_topic_endpoint_info_t has a member qos_profile of type const rmw_qos_profile_t *. - * This function assigns the passed qos_profile pointer to the member. - * - * If a non RMW_RET_OK return value is returned, the RMW error message will be set - * - * \param[inout] topic_endpoint_info pointer to an initialized instance of rmw_topic_endpoint_info_t - * \param[in] qos_profile the qos_profile to set in rmw_topic_endpoint_info_t - * \returns `RMW_RET_OK` on successfully setting the qos_profile, or - * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or - * \returns `RMW_RET_ERROR` when an unspecified error occurs. - */ -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_ret_t -rmw_topic_endpoint_info_set_qos_profile( - rmw_topic_endpoint_info_t * topic_endpoint_info, - const rmw_qos_profile_t * qos_profile); - -#ifdef __cplusplus -} -#endif - -#endif /* !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON */ - -#endif // RMW_CONNEXTDDS__TOPIC_ENDPOINT_INFO_H_ diff --git a/rmw_connextdds_common/include/rmw_connextdds/topic_endpoint_info_array.h b/rmw_connextdds_common/include/rmw_connextdds/topic_endpoint_info_array.h deleted file mode 100644 index 00c335d1..00000000 --- a/rmw_connextdds_common/include/rmw_connextdds/topic_endpoint_info_array.h +++ /dev/null @@ -1,121 +0,0 @@ -// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. -// -// 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 RMW_CONNEXTDDS__TOPIC_ENDPOINT_INFO_ARRAY_H_ -#define RMW_CONNEXTDDS__TOPIC_ENDPOINT_INFO_ARRAY_H_ - -#include "rmw_connextdds/static_config.hpp" - -#if !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON - -#ifdef __cplusplus -extern "C" -{ -#endif - -#include "rcutils/allocator.h" -#include "rmw_connextdds/topic_endpoint_info.h" -#include "rmw/visibility_control.h" - -/// Array of rmw_topic_endpoint_info_t -typedef struct RMW_PUBLIC_TYPE rmw_topic_endpoint_info_array_t -{ - /// Size of the array. - size_t size; - /// Pointer representing an array of rmw_topic_endpoint_info_t - rmw_topic_endpoint_info_t * info_array; -} rmw_topic_endpoint_info_array_t; - -/// Return a rmw_topic_endpoint_info_array_t struct with members initialized to `NULL`. -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_topic_endpoint_info_array_t -rmw_get_zero_initialized_topic_endpoint_info_array(void); - -/// Check that a rmw_topic_endpoint_info_array_t struct is zero initialized. -/** - * This function checks if the provided rmw_topic_endpoint_info_array_t is zero initialized or not. - * - * If a non RMW_RET_OK return value is returned, the RMW error message will be set - * - * \param[in] topic_endpoint_info_array the data structure to be checked - * \returns `RMW_RET_OK` if topic_endpoint_info_array is zero initialized - * \returns `RMW_RET_INVALID_ARGUMENT` if the parameter is NULL, or - * \returns `RMW_RET_ERROR` if topic_endpoint_info_array is not zero initialized - */ -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_ret_t -rmw_topic_endpoint_info_array_check_zero( - rmw_topic_endpoint_info_array_t * topic_endpoint_info_array); - -/// Initialize the info_array member inside rmw_topic_endpoint_info_array_t with the given size -/** - * The rmw_topic_endpoint_info_array_t has a member variable info_array which is an array of - * type rmw_topic_endpoint_info_t. - * This function allocates memory to this array to hold n elements, - * where n is the value of the size param to this function. - * The member `size` is updated accordingly. - * - * topic_endpoint_info_array must be zero initialized before being passed into this function. - * - * If a non RMW_RET_OK return value is returned, the RMW error message will be set - * - * \param[inout] topic_endpoint_info_array the data structure to initialise - * \param[in] size the size of the array - * \param[in] allocator the allocator to be used to allocate space - * \returns `RMW_RET_OK` on successful init, or - * \returns `RMW_RET_INVALID_ARGUMENT` if any of the parameters are NULL, or - * \returns `RMW_RET_INVALID_ARGUMENT` if topic_endpoint_info_array is not zero initialized, or - * \returns `RMW_BAD_ALLOC` if memory allocation fails, or - * \returns `RMW_RET_ERROR` when an unspecified error occurs. - */ -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_ret_t -rmw_topic_endpoint_info_array_init_with_size( - rmw_topic_endpoint_info_array_t * topic_endpoint_info_array, - size_t size, - rcutils_allocator_t * allocator); - -/// Finalize a rmw_topic_endpoint_info_array_t object. -/** - * The info_array member variable inside of rmw_topic_endpoint_info_array represents an array of - * rmw_topic_endpoint_info_t. - * When initializing this array, memory is allocated for it using the allocator. - * This function reclaims any allocated resources within the object and also sets the value of size - * to 0. - * - * If a non RMW_RET_OK return value is returned, the RMW error message will be set - * - * \param[inout] topic_endpoint_info_array object to be finalized - * \param[in] allocator the allocator used to allocate memory to the object - * \returns `RMW_RET_OK` on successfully reclaiming memory, or - * \returns `RMW_RET_INVALID_ARGUMENT` if any parameters are NULL, or - * \returns `RMW_RET_ERROR` when an unspecified error occurs. - */ -RMW_PUBLIC -RMW_WARN_UNUSED -rmw_ret_t -rmw_topic_endpoint_info_array_fini( - rmw_topic_endpoint_info_array_t * topic_endpoint_info_array, - rcutils_allocator_t * allocator); - -#ifdef __cplusplus -} -#endif - -#endif /* !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON */ - -#endif // RMW_CONNEXTDDS__TOPIC_ENDPOINT_INFO_ARRAY_H_ diff --git a/rmw_connextdds_common/package.xml b/rmw_connextdds_common/package.xml index 95852dfb..8cdccaa8 100644 --- a/rmw_connextdds_common/package.xml +++ b/rmw_connextdds_common/package.xml @@ -24,8 +24,6 @@ ament_lint_auto ament_lint_common - rosidl_interface_packages - ament_cmake diff --git a/rmw_connextdds_common/rmw_connextdds_common-extras.cmake.in b/rmw_connextdds_common/rmw_connextdds_common-extras.cmake.in deleted file mode 100644 index 47e5dc69..00000000 --- a/rmw_connextdds_common/rmw_connextdds_common-extras.cmake.in +++ /dev/null @@ -1,27 +0,0 @@ -################################################################################ -# -# (c) 2020 Copyright, Real-Time Innovations, Inc. (RTI) -# -# 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. -# -################################################################################ - -# Generated from @PROJECT_NAME@/cmake/@PROJECT_NAME@-extras.cmake.in - -set(RMW_CONNEXT_RELEASE "@RMW_CONNEXT_RELEASE@" - CACHE INTERNAL "Target ROS release against which @PROJECT_NAME was built.") - -set(RMW_CONNEXT_PROVIDE_RMW_DDS_COMMON @RMW_CONNEXT_PROVIDE_RMW_DDS_COMMON@ - CACHE INTERNAL - "Whether the rmw_dds_common package is available in the target release.") - diff --git a/rmw_connextdds_common/src/common/rmw_context.cpp b/rmw_connextdds_common/src/common/rmw_context.cpp index ec183057..5fb50d5b 100644 --- a/rmw_connextdds_common/src/common/rmw_context.cpp +++ b/rmw_connextdds_common/src/common/rmw_context.cpp @@ -636,16 +636,6 @@ rmw_api_connextdds_init_options_init( init_options->implementation_identifier = RMW_CONNEXTDDS_ID; init_options->allocator = allocator; init_options->impl = nullptr; -#if RMW_CONNEXT_HAVE_LOCALHOST_ONLY - init_options->localhost_only = RMW_LOCALHOST_ONLY_DEFAULT; -#endif /* RMW_CONNEXT_HAVE_LOCALHOST_ONLY */ -#if RMW_CONNEXT_HAVE_OPTIONS - init_options->domain_id = RMW_DEFAULT_DOMAIN_ID; - init_options->enclave = nullptr; -#endif /* RMW_CONNEXT_HAVE_OPTIONS */ -#if RMW_CONNEXT_HAVE_SECURITY - init_options->security_options = rmw_get_zero_initialized_security_options(); -#endif /* RMW_CONNEXT_HAVE_SECURITY */ return RMW_RET_OK; } @@ -673,24 +663,6 @@ rmw_api_connextdds_init_options_copy( rmw_init_options_t tmp = *src; -#if RMW_CONNEXT_HAVE_OPTIONS || RMW_CONNEXT_HAVE_SECURITY -#if RMW_CONNEXT_HAVE_OPTIONS - const rcutils_allocator_t * allocator = &src->allocator; - tmp.enclave = rcutils_strdup(tmp.enclave, *allocator); - if (nullptr != src->enclave && nullptr == tmp.enclave) { - return RMW_RET_BAD_ALLOC; - } -#endif /* RMW_CONNEXT_HAVE_OPTIONS */ -#if RMW_CONNEXT_HAVE_SECURITY - tmp.security_options = rmw_get_zero_initialized_security_options(); - rmw_ret_t ret = - rmw_security_options_copy(&src->security_options, allocator, &tmp.security_options); - if (RMW_RET_OK != ret) { - allocator->deallocate(tmp.enclave, allocator->state); - return ret; - } -#endif /* RMW_CONNEXT_HAVE_SECURITY */ -#endif /* RMW_CONNEXT_HAVE_OPTIONS || RMW_CONNEXT_HAVE_SECURITY */ *dst = tmp; return RMW_RET_OK; } @@ -710,18 +682,7 @@ rmw_api_connextdds_init_options_fini(rmw_init_options_t * init_options) RMW_CONNEXTDDS_ID, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); -#if RMW_CONNEXT_HAVE_OPTIONS || RMW_CONNEXT_HAVE_SECURITY - rcutils_allocator_t * allocator = &init_options->allocator; - RCUTILS_CHECK_ALLOCATOR(allocator, return RMW_RET_INVALID_ARGUMENT); -#if RMW_CONNEXT_HAVE_OPTIONS - allocator->deallocate(init_options->enclave, allocator->state); -#endif /* RMW_CONNEXT_HAVE_OPTIONS */ -#if RMW_CONNEXT_HAVE_SECURITY - rmw_ret_t ret = rmw_security_options_fini(&init_options->security_options, allocator); -#endif /* RMW_CONNEXT_HAVE_SECURITY */ -#else rmw_ret_t ret = RMW_RET_OK; -#endif /* RMW_CONNEXT_HAVE_OPTIONS || RMW_CONNEXT_HAVE_SECURITY */ *init_options = rmw_get_zero_initialized_init_options(); return ret; } @@ -743,26 +704,12 @@ rmw_api_connextdds_init( options->implementation_identifier, RMW_CONNEXTDDS_ID, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); -#if RMW_CONNEXT_HAVE_OPTIONS - RMW_CHECK_FOR_NULL_WITH_MSG( - options->enclave, - "expected non-null enclave", - return RMW_RET_INVALID_ARGUMENT); -#endif /* RMW_CONNEXT_HAVE_OPTIONS */ + if (nullptr != context->implementation_identifier) { RMW_CONNEXT_LOG_ERROR_SET("expected a zero-initialized context") return RMW_RET_INVALID_ARGUMENT; } -#if RMW_CONNEXT_HAVE_OPTIONS - if (options->domain_id >= INT32_MAX && - options->domain_id != RMW_DEFAULT_DOMAIN_ID) - { - RMW_CONNEXT_LOG_ERROR_SET("domain id out of range") - return RMW_RET_INVALID_ARGUMENT; - } -#endif /* RMW_CONNEXT_HAVE_OPTIONS */ - auto scope_exit_context_reset = rcpputils::make_scope_exit( [context]() { @@ -771,34 +718,7 @@ rmw_api_connextdds_init( context->instance_id = options->instance_id; context->implementation_identifier = RMW_CONNEXTDDS_ID; - const DDS_DomainId_t actual_domain_id = -#if !RMW_CONNEXT_HAVE_OPTIONS - RMW_CONNEXT_DEFAULT_DOMAIN; -#else - (RMW_DEFAULT_DOMAIN_ID != options->domain_id) ? - static_cast(options->domain_id) : RMW_CONNEXT_DEFAULT_DOMAIN; -#if RMW_CONNEXT_HAVE_GET_DOMAIN - context->actual_domain_id = actual_domain_id; -#endif /* RMW_CONNEXT_HAVE_GET_DOMAIN */ - rmw_ret_t rc = rmw_api_connextdds_init_options_copy(options, &context->options); - if (RMW_RET_OK != rc) { - RMW_CONNEXT_LOG_ERROR("failed to copy RMW context options") - return rc; - } - - auto scope_exit_context_opts_finalize = - rcpputils::make_scope_exit( - [context]() - { - if (RMW_RET_OK != rmw_api_connextdds_init_options_fini(&context->options)) { - RMW_CONNEXT_LOG_ERROR("failed to finalize RMW context options") - } - }); - -#endif /* RMW_CONNEXT_HAVE_OPTIONS*/ - - /* The context object will be initialized upon creation of the first node */ - + const DDS_DomainId_t actual_domain_id = RMW_CONNEXT_DEFAULT_DOMAIN; rmw_context_impl_t * const ctx = new (std::nothrow) rmw_context_impl_t(context); if (nullptr == ctx) { @@ -985,9 +905,6 @@ rmw_api_connextdds_init( RMW_CONNEXT_ASSERT(1 == RMW_Connext_gv_ContextCount) scope_exit_context_finalize.cancel(); -#if RMW_CONNEXT_HAVE_OPTIONS - scope_exit_context_opts_finalize.cancel(); -#endif /* RMW_CONNEXT_HAVE_OPTIONS */ scope_exit_context_reset.cancel(); return RMW_RET_OK; @@ -1049,13 +966,6 @@ rmw_api_connextdds_context_fini(rmw_context_t * context) rc_exit = rc; } -#if RMW_CONNEXT_HAVE_OPTIONS - rc = rmw_api_connextdds_init_options_fini(&context->options); - if (RMW_RET_OK != rc) { - RMW_CONNEXT_LOG_ERROR("failed to finalize RMW context options") - rc_exit = rc; - } -#endif /* RMW_CONNEXT_HAVE_OPTIONS */ delete context->impl; *context = rmw_get_zero_initialized_context(); return rc_exit; @@ -1066,198 +976,8 @@ rmw_connextdds_configure_security( rmw_context_impl_t * const ctx, DDS_DomainParticipantQos * const qos) { -#if RMW_CONNEXT_HAVE_SECURITY - if (nullptr == ctx->base->options.security_options.security_root_path) { - // Security not enabled; - return RMW_RET_OK; - } - - rmw_ret_t rc = rmw_connextdds_enable_security(ctx, qos); - if (RMW_RET_OK != rc) { - return rc; - } - - rcutils_allocator_t allocator = rcutils_get_default_allocator(); - rcutils_allocator_t * const allocator_ptr = &allocator; - std::ostringstream ss; - std::string prop_uri; -#if !RMW_CONNEXT_DDS_API_PRO_LEGACY - static const char * const uri_prefix = "file:"; -#else - // Connext Pro 5.3.1 does not support the "file:" prefix - static const char * const uri_prefix = ""; -#endif /* !RMW_CONNEXT_DDS_API_PRO_LEGACY */ - - char * const prop_identity_ca = - rcutils_join_path( - ctx->base->options.security_options.security_root_path, - "identity_ca.cert.pem", - allocator); - auto scope_exit_prop_identity_ca = rcpputils::make_scope_exit( - [allocator_ptr, prop_identity_ca]() - { - allocator_ptr->deallocate(prop_identity_ca, allocator_ptr->state); - }); - ss << uri_prefix << prop_identity_ca; - prop_uri = ss.str(); - ss.str(""); - /* X509 Certificate of the Identity CA */ - if (DDS_RETCODE_OK != - DDS_PropertyQosPolicyHelper_assert_property( - &qos->property, - DDS_SECURITY_IDENTITY_CA_PROPERTY, - prop_uri.c_str(), - RTI_FALSE)) - { - RMW_CONNEXT_LOG_ERROR_A_SET( - "failed to assert DDS property: '%s' = '%s'", - DDS_SECURITY_IDENTITY_CA_PROPERTY, prop_uri.c_str()) - return RMW_RET_ERROR; - } - - char * const prop_perm_ca = - rcutils_join_path( - ctx->base->options.security_options.security_root_path, - "permissions_ca.cert.pem", - allocator); - auto scope_exit_prop_perm_ca = rcpputils::make_scope_exit( - [allocator_ptr, prop_perm_ca]() - { - allocator_ptr->deallocate(prop_perm_ca, allocator_ptr->state); - }); - ss << uri_prefix << prop_perm_ca; - prop_uri = ss.str(); - ss.str(""); - /* X509 Certificate of the Permissions CA */ - if (DDS_RETCODE_OK != - DDS_PropertyQosPolicyHelper_assert_property( - &qos->property, - DDS_SECURITY_PERMISSIONS_CA_PROPERTY, - prop_uri.c_str(), - RTI_FALSE)) - { - RMW_CONNEXT_LOG_ERROR_A_SET( - "failed to assert DDS property: '%s' = '%s'", - DDS_SECURITY_PERMISSIONS_CA_PROPERTY, prop_uri.c_str()) - return RMW_RET_ERROR; - } - - char * const prop_peer_key = - rcutils_join_path( - ctx->base->options.security_options.security_root_path, - "key.pem", - allocator); - auto scope_exit_prop_peer_key = rcpputils::make_scope_exit( - [allocator_ptr, prop_peer_key]() - { - allocator_ptr->deallocate(prop_peer_key, allocator_ptr->state); - }); - ss << uri_prefix << prop_peer_key; - prop_uri = ss.str(); - ss.str(""); - /* Private Key of the DomainParticipant's identity */ - if (DDS_RETCODE_OK != - DDS_PropertyQosPolicyHelper_assert_property( - &qos->property, - DDS_SECURITY_PRIVATE_KEY_PROPERTY, - prop_uri.c_str(), - RTI_FALSE)) - { - RMW_CONNEXT_LOG_ERROR_A_SET( - "failed to assert DDS property: '%s' = '%s'", - DDS_SECURITY_PRIVATE_KEY_PROPERTY, prop_uri.c_str()) - return RMW_RET_ERROR; - } - - char * const prop_peer_cert = - rcutils_join_path( - ctx->base->options.security_options.security_root_path, - "cert.pem", - allocator); - auto scope_exit_prop_peer_cert = rcpputils::make_scope_exit( - [allocator_ptr, prop_peer_cert]() - { - allocator_ptr->deallocate(prop_peer_cert, allocator_ptr->state); - }); - ss << uri_prefix << prop_peer_cert; - prop_uri = ss.str(); - ss.str(""); - /* Public certificate of the DomainParticipant's identity, signed - * by the Certificate Authority */ - if (DDS_RETCODE_OK != - DDS_PropertyQosPolicyHelper_assert_property( - &qos->property, - DDS_SECURITY_IDENTITY_CERTIFICATE_PROPERTY, - prop_uri.c_str(), - RTI_FALSE)) - { - RMW_CONNEXT_LOG_ERROR_A_SET( - "failed to assert DDS property: '%s' = '%s'", - DDS_SECURITY_IDENTITY_CERTIFICATE_PROPERTY, prop_uri.c_str()) - return RMW_RET_ERROR; - } - - char * const prop_governance = - rcutils_join_path( - ctx->base->options.security_options.security_root_path, - "governance.p7s", - allocator); - auto scope_exit_prop_governance = rcpputils::make_scope_exit( - [allocator_ptr, prop_governance]() - { - allocator_ptr->deallocate(prop_governance, allocator_ptr->state); - }); - ss << uri_prefix << prop_governance; - prop_uri = ss.str(); - ss.str(""); - /* XML file containing domain governance configuration, signed by - * the Permission CA */ - if (DDS_RETCODE_OK != - DDS_PropertyQosPolicyHelper_assert_property( - &qos->property, - DDS_SECURITY_GOVERNANCE_PROPERTY, - prop_uri.c_str(), - RTI_FALSE)) - { - RMW_CONNEXT_LOG_ERROR_A_SET( - "failed to assert DDS property: '%s' = '%s'", - DDS_SECURITY_GOVERNANCE_PROPERTY, prop_uri.c_str()) - return RMW_RET_ERROR; - } - - char * const prop_permissions = - rcutils_join_path( - ctx->base->options.security_options.security_root_path, - "permissions.p7s", - allocator); - auto scope_exit_prop_permissions = rcpputils::make_scope_exit( - [allocator_ptr, prop_permissions]() - { - allocator_ptr->deallocate(prop_permissions, allocator_ptr->state); - }); - ss << uri_prefix << prop_permissions; - prop_uri = ss.str(); - ss.str(""); - /* XML file containing domain permissions configuration, signed by - * the Permission CA */ - if (DDS_RETCODE_OK != - DDS_PropertyQosPolicyHelper_assert_property( - &qos->property, - DDS_SECURITY_PERMISSIONS_PROPERTY, - prop_uri.c_str(), - RTI_FALSE)) - { - RMW_CONNEXT_LOG_ERROR_A_SET( - "failed to assert DDS property: '%s' = '%s'", - DDS_SECURITY_PERMISSIONS_PROPERTY, prop_uri.c_str()) - return RMW_RET_ERROR; - } - - return rmw_connextdds_apply_security_logging_configuration(&qos->property); -#else // Security not supported by ROS release UNUSED_ARG(ctx); UNUSED_ARG(qos); return RMW_RET_OK; -#endif /* RMW_CONNEXT_HAVE_SECURITY */ } diff --git a/rmw_connextdds_common/src/common/rmw_graph.cpp b/rmw_connextdds_common/src/common/rmw_graph.cpp index c3d63c78..6449989d 100644 --- a/rmw_connextdds_common/src/common/rmw_graph.cpp +++ b/rmw_connextdds_common/src/common/rmw_graph.cpp @@ -21,7 +21,7 @@ #define rmw_publish rmw_api_connextdds_publish static rmw_ret_t - rmw_connextdds_graph_add_entityEA( +rmw_connextdds_graph_add_entityEA( rmw_context_impl_t * ctx, const DDS_GUID_t * const endp_guid, const DDS_GUID_t * const dp_guid, @@ -31,9 +31,6 @@ static rmw_ret_t const DDS_DurabilityQosPolicy * const durability, const DDS_DeadlineQosPolicy * const deadline, const DDS_LivelinessQosPolicy * const liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - const DDS_LifespanQosPolicy * const lifespan, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ const bool is_reader, const bool local); @@ -65,7 +62,6 @@ rmw_connextdds_graph_initialize(rmw_context_impl_t * const ctx) pubsub_qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; pubsub_qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB /* Create RMW publisher/subscription/guard condition used by rmw_dds_common discovery */ rmw_publisher_options_t publisher_options = @@ -73,15 +69,10 @@ rmw_connextdds_graph_initialize(rmw_context_impl_t * const ctx) rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); subscription_options.ignore_local_publications = true; -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const rosidl_message_type_support_t * const type_supports_partinfo = rosidl_typesupport_cpp::get_message_type_support_handle< -#if RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON rmw_dds_common::msg::ParticipantEntitiesInfo>(); -#else - rmw_connextdds_common::msg::ParticipantEntitiesInfo > (); -#endif /* RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON */ const char * const topic_name_partinfo = "ros_discovery_info"; @@ -96,9 +87,7 @@ rmw_connextdds_graph_initialize(rmw_context_impl_t * const ctx) type_supports_partinfo, topic_name_partinfo, &pubsub_qos, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB &publisher_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ true /* internal */); if (nullptr == ctx->common.pub) { @@ -120,11 +109,7 @@ rmw_connextdds_graph_initialize(rmw_context_impl_t * const ctx) type_supports_partinfo, topic_name_partinfo, &pubsub_qos, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB &subscription_options, -#else - true /* ignore_local_publications */, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ true /* internal */); if (nullptr == ctx->common.sub) { RMW_CONNEXT_LOG_ERROR( @@ -156,10 +141,6 @@ rmw_connextdds_graph_initialize(rmw_context_impl_t * const ctx) std::string dp_enclave; -#if RMW_CONNEXT_HAVE_OPTIONS - dp_enclave = ctx->base->options.enclave; -#endif /* RMW_CONNEXT_HAVE_OPTIONS*/ - ctx->common.graph_cache.add_participant(ctx->common.gid, dp_enclave); if (RMW_RET_OK != @@ -748,9 +729,6 @@ rmw_connextdds_graph_add_entityEA( const DDS_DurabilityQosPolicy * const durability, const DDS_DeadlineQosPolicy * const deadline, const DDS_LivelinessQosPolicy * const liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - const DDS_LifespanQosPolicy * const lifespan, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ const bool is_reader, const bool local) { @@ -769,9 +747,6 @@ rmw_connextdds_graph_add_entityEA( durability, deadline, liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - lifespan, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ &qos_profile)) { // this should never happen with a valid DDS implementation @@ -899,13 +874,6 @@ rmw_connextdds_graph_add_local_publisherEA( &dw_qos.durability, &dw_qos.deadline, &dw_qos.liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS -#if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO - &dw_qos.lifespan, -#elif RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_MICRO - nullptr /* Micro doesn't support LifespanQosPolicy */, -#endif /* RMW_CONNEXT_DDS_API */ -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ false /* is_reader */, true /* local */); } @@ -971,9 +939,6 @@ rmw_connextdds_graph_add_local_subscriberEA( &dr_qos.durability, &dr_qos.deadline, &dr_qos.liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - nullptr /* Lifespan is a writer-only qos policy */, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ true /* is_reader */, true /* local */); } @@ -989,9 +954,6 @@ rmw_connextdds_graph_add_remote_entity( const DDS_DurabilityQosPolicy * const durability, const DDS_DeadlineQosPolicy * const deadline, const DDS_LivelinessQosPolicy * const liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - const DDS_LifespanQosPolicy * const lifespan, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ const bool is_reader) { std::lock_guard guard(ctx->common.node_update_mutex); @@ -1016,9 +978,6 @@ rmw_connextdds_graph_add_remote_entity( durability, deadline, liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - lifespan, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ is_reader, false /* local */); if (RMW_RET_OK != rc) { diff --git a/rmw_connextdds_common/src/common/rmw_impl.cpp b/rmw_connextdds_common/src/common/rmw_impl.cpp index ecd794af..8e836978 100644 --- a/rmw_connextdds_common/src/common/rmw_impl.cpp +++ b/rmw_connextdds_common/src/common/rmw_impl.cpp @@ -255,25 +255,16 @@ rmw_connextdds_get_readerwriter_qos( DDS_LivelinessQosPolicy * const liveliness, DDS_ResourceLimitsQosPolicy * const resource_limits, DDS_PublishModeQosPolicy * const publish_mode, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - DDS_LifespanQosPolicy * const lifespan, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ - const rmw_qos_profile_t * const qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , + const rmw_qos_profile_t * const qos_policies, const rmw_publisher_options_t * const pub_options, - const rmw_subscription_options_t * const sub_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -) + const rmw_subscription_options_t * const sub_options) { UNUSED_ARG(writer_qos); UNUSED_ARG(type_support); UNUSED_ARG(publish_mode); UNUSED_ARG(resource_limits); -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB UNUSED_ARG(pub_options); UNUSED_ARG(sub_options); -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ switch (qos_policies->history) { case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: @@ -409,35 +400,6 @@ rmw_connextdds_get_readerwriter_qos( } } -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - if (nullptr != lifespan && - (qos_policies->lifespan.sec != 0 || qos_policies->lifespan.nsec != 0)) - { -#if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO - lifespan->duration.sec = static_cast(qos_policies->lifespan.sec); - lifespan->duration.nanosec = - static_cast(qos_policies->lifespan.nsec); -#else - RMW_CONNEXT_LOG_WARNING("lifespan qos policy not supported") -#endif /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO */ - } -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ - - // Make sure that resource limits are consistent with history qos - // TODO(asorbini): do not overwrite if using non-default QoS - // if (history->kind == DDS_KEEP_LAST_HISTORY_QOS && - // history->depth > 1 && - // resource_limits->max_samples_per_instance == DDS_LENGTH_UNLIMITED) - // { - // resource_limits->max_samples_per_instance = history->depth; - // if (resource_limits->max_samples != DDS_LENGTH_UNLIMITED && - // resource_limits->max_samples < resource_limits->max_samples_per_instance) - // { - // resource_limits->max_samples = - // resource_limits->max_samples_per_instance; - // } - // } - return RMW_RET_OK; } @@ -448,9 +410,6 @@ rmw_connextdds_readerwriter_qos_to_ros( const DDS_DurabilityQosPolicy * const durability, const DDS_DeadlineQosPolicy * const deadline, const DDS_LivelinessQosPolicy * const liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - const DDS_LifespanQosPolicy * const lifespan, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ rmw_qos_profile_t * const qos_policies) { if (nullptr != history) { @@ -540,17 +499,6 @@ rmw_connextdds_readerwriter_qos_to_ros( } } -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - if (nullptr != lifespan) { -#if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO - qos_policies->lifespan.sec = lifespan->duration.sec; - qos_policies->lifespan.nsec = lifespan->duration.nanosec; -#else - RMW_CONNEXT_LOG_WARNING("lifespan qos policy not supported") -#endif /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO */ - } -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ - return RMW_RET_OK; } @@ -566,13 +514,6 @@ rmw_connextdds_datawriter_qos_to_ros( &qos->durability, &qos->deadline, &qos->liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS -#if RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO - &qos->lifespan, -#elif RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_MICRO - nullptr, -#endif /* RMW_CONNEXT_DDS_API == RMW_CONNEXT_DDS_API_PRO */ -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ qos_policies); } @@ -588,9 +529,6 @@ rmw_connextdds_datareader_qos_to_ros( &qos->durability, &qos->deadline, &qos->liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - nullptr /* Lifespan is a writer-only qos policy */, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ qos_policies); } @@ -651,9 +589,7 @@ RMW_Connext_Publisher::create( const rosidl_message_type_support_t * const type_supports, const char * const topic_name, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_publisher_options_t * const publisher_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal, const RMW_Connext_MessageType msg_type, const void * const intro_members, @@ -773,9 +709,7 @@ RMW_Connext_Publisher::create( dp, pub, qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB publisher_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ internal, type_support, topic, @@ -980,9 +914,7 @@ rmw_connextdds_create_publisher( const rosidl_message_type_support_t * const type_supports, const char * const topic_name, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_publisher_options_t * const publisher_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal) { std::lock_guard guard(ctx->endpoint_mutex); @@ -994,9 +926,7 @@ rmw_connextdds_create_publisher( type_supports, topic_name, qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB publisher_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ internal); if (nullptr == rmw_pub_impl) { @@ -1047,12 +977,7 @@ rmw_connextdds_create_publisher( const_cast(rmw_publisher->topic_name), topic_name, topic_name_len + 1); -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB rmw_publisher->options = *publisher_options; -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -#if RMW_CONNEXT_HAVE_LOAN_MESSAGE - rmw_publisher->can_loan_messages = false; -#endif /* RMW_CONNEXT_HAVE_LOAN_MESSAGE */ if (!internal) { if (RMW_RET_OK != rmw_pub_impl->enable()) { @@ -1152,11 +1077,7 @@ RMW_Connext_Subscriber::create( const rosidl_message_type_support_t * const type_supports, const char * const topic_name, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_subscription_options_t * const subscriber_options, -#else - const bool ignore_local_publications, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal, const RMW_Connext_MessageType msg_type, const void * const intro_members, @@ -1298,9 +1219,7 @@ RMW_Connext_Subscriber::create( dp, sub, qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB subscriber_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ internal, type_support, sub_topic, @@ -1329,11 +1248,7 @@ RMW_Connext_Subscriber::create( dds_reader, topic, type_support, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB subscriber_options->ignore_local_publications, -#else - ignore_local_publications, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ topic_created, cft_topic, internal); @@ -1485,29 +1400,6 @@ RMW_Connext_Subscriber::take_message( return rc; } -#if RMW_CONNEXT_HAVE_TAKE_SEQ -rmw_ret_t -RMW_Connext_Subscriber::take( - rmw_message_sequence_t * const message_sequence, - rmw_message_info_sequence_t * const message_info_sequence, - const size_t max_samples, - size_t * const taken) -{ - if (max_samples == 0 || - message_sequence->capacity < max_samples || - message_info_sequence->capacity != message_sequence->capacity) - { - return RMW_RET_INVALID_ARGUMENT; - } - return this->take_next( - message_sequence->data, - message_info_sequence->data, - max_samples, - taken, - false /* serialized*/); -} -#endif /* RMW_CONNEXT_HAVE_TAKE_SEQ */ - rmw_ret_t RMW_Connext_Subscriber::take_serialized( rmw_serialized_message_t * const serialized_message, @@ -1750,11 +1642,7 @@ rmw_connextdds_create_subscriber( const rosidl_message_type_support_t * const type_supports, const char * const topic_name, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_subscription_options_t * const subscriber_options, -#else - const bool ignore_local_publications, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal) { UNUSED_ARG(internal); @@ -1768,11 +1656,7 @@ rmw_connextdds_create_subscriber( type_supports, topic_name, qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB subscriber_options, -#else - ignore_local_publications, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ internal); if (nullptr == rmw_sub_impl) { @@ -1822,12 +1706,7 @@ rmw_connextdds_create_subscriber( const_cast(rmw_subscriber->topic_name), topic_name, topic_name_len + 1); -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB rmw_subscriber->options = *subscriber_options; -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -#if RMW_CONNEXT_HAVE_LOAN_MESSAGE - rmw_subscriber->can_loan_messages = false; -#endif /* RMW_CONNEXT_HAVE_LOAN_MESSAGE */ if (!internal) { if (RMW_RET_OK != rmw_sub_impl->enable()) { @@ -1889,17 +1768,6 @@ rmw_connextdds_message_info_from_dds( const DDS_SampleInfo * const from) { rmw_connextdds_ih_to_gid(from->publication_handle, to->publisher_gid); -#if RMW_CONNEXT_HAVE_MESSAGE_INFO_TS -// Message timestamps are disabled on Windows because RTI Connext DDS -// does not support a high enough clock resolution by default (see: _ftime()). -#if !RTI_WIN32 - to->source_timestamp = dds_time_to_u64(&from->source_timestamp); - to->received_timestamp = dds_time_to_u64(&from->reception_timestamp); -#else - to->source_timestamp = 0; - to->received_timestamp = 0; -#endif /* !RTI_WIN32 */ -#endif /* RMW_CONNEXT_HAVE_MESSAGE_INFO_TS */ } /****************************************************************************** @@ -2379,11 +2247,8 @@ RMW_Connext_Client::create( RMW_Connext_ServiceTypeSupportWrapper::get_response_type_name( type_supports); -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB rmw_publisher_options_t pub_options = rmw_get_default_publisher_options(); - rmw_subscription_options_t sub_options = - rmw_get_default_subscription_options(); -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ + rmw_subscription_options_t sub_options = rmw_get_default_subscription_options(); RMW_CONNEXT_LOG_DEBUG_A( @@ -2401,9 +2266,7 @@ RMW_Connext_Client::create( type_support_req, request_topic.c_str(), qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB &pub_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ false /* internal */, RMW_CONNEXT_MESSAGE_REQUEST, svc_members_req, @@ -2453,11 +2316,7 @@ RMW_Connext_Client::create( type_support_res, reply_topic.c_str(), qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB &sub_options, -#else - false /* ignore_local_publications */, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ false /* internal */, RMW_CONNEXT_MESSAGE_REPLY, svc_members_res, @@ -2543,11 +2402,6 @@ RMW_Connext_Client::take_response( 8); } -#if RMW_CONNEXT_HAVE_MESSAGE_INFO_TS - request_header->source_timestamp = message_info.source_timestamp; - request_header->received_timestamp = message_info.received_timestamp; -#endif /* RMW_CONNEXT_HAVE_MESSAGE_INFO_TS */ - *taken = true; RMW_CONNEXT_LOG_DEBUG_A( @@ -2706,11 +2560,8 @@ RMW_Connext_Service::create( RMW_Connext_ServiceTypeSupportWrapper::get_response_type_name( type_supports); -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB rmw_publisher_options_t pub_options = rmw_get_default_publisher_options(); - rmw_subscription_options_t sub_options = - rmw_get_default_subscription_options(); -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ + rmw_subscription_options_t sub_options = rmw_get_default_subscription_options(); RMW_CONNEXT_LOG_DEBUG_A( "creating reply publisher: " @@ -2727,9 +2578,7 @@ RMW_Connext_Service::create( type_support_res, reply_topic.c_str(), qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB &pub_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ false /* internal */, RMW_CONNEXT_MESSAGE_REPLY, svc_members_res, @@ -2756,11 +2605,7 @@ RMW_Connext_Service::create( type_support_req, request_topic.c_str(), qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB &sub_options, -#else - false /* ignore_local_publications */, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ false /* internal */, RMW_CONNEXT_MESSAGE_REQUEST, svc_members_req, @@ -2822,10 +2667,6 @@ RMW_Connext_Service::take_request( request_header->request_id.writer_guid, rr_msg.gid.data, 16); -#if RMW_CONNEXT_HAVE_MESSAGE_INFO_TS - request_header->source_timestamp = message_info.source_timestamp; - request_header->received_timestamp = message_info.received_timestamp; -#endif /* RMW_CONNEXT_HAVE_MESSAGE_INFO_TS */ *taken = true; @@ -2923,34 +2764,8 @@ ros_event_to_dds(const rmw_event_type_t ros, bool * const invalid) { return DDS_OFFERED_DEADLINE_MISSED_STATUS; } -#if RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS_EVENT - case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: - { - return DDS_REQUESTED_INCOMPATIBLE_QOS_STATUS; - } - case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: - { - return DDS_OFFERED_INCOMPATIBLE_QOS_STATUS; - } -#endif /* RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS_EVENT */ -// Avoid warnings caused by RMW_EVENT_MESSAGE_LOST not being one of -// the defined values for rmw_event_type_t. This #if and the one in -// the `default` case, should be removed once support for releases -// without RMW_EVENT_MESSAGE_LOST is dropped (or the value is backported). -#if RMW_CONNEXT_HAVE_MESSAGE_LOST - case RMW_EVENT_MESSAGE_LOST: - { - return DDS_SAMPLE_LOST_STATUS; - } -#endif /* RMW_CONNEXT_HAVE_MESSAGE_LOST */ default: { -#if !RMW_CONNEXT_HAVE_MESSAGE_LOST - if (RMW_EVENT_MESSAGE_LOST == ros) { - RMW_CONNEXT_LOG_WARNING( - "unexpected rmw_event_type_t: RMW_EVENT_MESSAGE_LOST") - } -#endif /* !RMW_CONNEXT_HAVE_MESSAGE_LOST */ if (nullptr != invalid) { *invalid = true; } @@ -3004,27 +2819,11 @@ ros_event_for_reader(const rmw_event_type_t ros) switch (ros) { case RMW_EVENT_LIVELINESS_CHANGED: case RMW_EVENT_REQUESTED_DEADLINE_MISSED: -#if RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS_EVENT - case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: -#endif /* RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS_EVENT */ -// Avoid warnings caused by RMW_EVENT_MESSAGE_LOST not being one of -// the defined values for rmw_event_type_t. This #if and the one in -// the `default` case, should be removed once support for releases -// without RMW_EVENT_MESSAGE_LOST is dropped (or the value is backported). -#if RMW_CONNEXT_HAVE_MESSAGE_LOST - case RMW_EVENT_MESSAGE_LOST: -#endif /* RMW_CONNEXT_HAVE_MESSAGE_LOST */ { return true; } default: { -#if !RMW_CONNEXT_HAVE_MESSAGE_LOST - if (RMW_EVENT_MESSAGE_LOST == ros) { - RMW_CONNEXT_LOG_WARNING( - "unexpected rmw_event_type_t: RMW_EVENT_MESSAGE_LOST") - } -#endif /* !RMW_CONNEXT_HAVE_MESSAGE_LOST */ return false; } } @@ -3053,30 +2852,6 @@ RMW_Connext_SubscriberStatusCondition::get_status( rc = this->get_requested_deadline_missed_status(status); break; } -#if RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS_EVENT - case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: - { - rmw_requested_qos_incompatible_event_status_t * const status = - reinterpret_cast(event_info); - - rc = this->get_requested_qos_incompatible_status(status); - break; - } -#endif /* RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS_EVENT */ -// Avoid warnings caused by RMW_EVENT_MESSAGE_LOST not being one of -// the defined values for rmw_event_type_t. This #if and the one in -// the `default` case, should be removed once support for releases -// without RMW_EVENT_MESSAGE_LOST is dropped (or the value is backported). -#if RMW_CONNEXT_HAVE_MESSAGE_LOST - case RMW_EVENT_MESSAGE_LOST: - { - rmw_message_lost_status_t * const status = - reinterpret_cast(event_info); - - rc = this->get_message_lost_status(status); - break; - } -#endif /* RMW_CONNEXT_HAVE_MESSAGE_LOST */ default: { RMW_CONNEXT_LOG_ERROR_A_SET( @@ -3112,16 +2887,6 @@ RMW_Connext_PublisherStatusCondition::get_status( rc = this->get_offered_deadline_missed_status(status); break; } -#if RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS_EVENT - case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: - { - rmw_offered_qos_incompatible_event_status_t * const status = - reinterpret_cast(event_info); - - rc = this->get_offered_qos_incompatible_status(status); - break; - } -#endif /* RMW_CONNEXT_HAVE_INCOMPATIBLE_QOS_EVENT */ default: { RMW_CONNEXT_LOG_ERROR_A_SET("unsupported publisher qos: %d", event_type) diff --git a/rmw_connextdds_common/src/common/rmw_impl_waitset_dds.cpp b/rmw_connextdds_common/src/common/rmw_impl_waitset_dds.cpp index 54febdc7..0e9b9720 100644 --- a/rmw_connextdds_common/src/common/rmw_impl_waitset_dds.cpp +++ b/rmw_connextdds_common/src/common/rmw_impl_waitset_dds.cpp @@ -16,10 +16,6 @@ #include "rmw_connextdds/rmw_impl.hpp" -#if RMW_CONNEXT_HAVE_TIME_UTILS -#include "rmw_dds_common/time_utils.hpp" -#endif /* RMW_CONNEXT_HAVE_TIME_UTILS */ - #if !RMW_CONNEXT_CPP_STD_WAITSETS /****************************************************************************** * WaitSet @@ -553,13 +549,9 @@ rmw_connextdds_duration_from_ros_time( DDS_Duration_t * const duration, const rmw_time_t * const ros_time) { -#if RMW_CONNEXT_HAVE_TIME_UTILS - rmw_time_t in_time = rmw_dds_common::clamp_rmw_time_to_dds_time(*ros_time); -#else // TODO(asorbini) In older versions, this function ignores possible overflows // which occur if (ros_time->sec > INT32_MAX || ros_time->nsec > UINT32_MAX) rmw_time_t in_time = *ros_time; -#endif /* RMW_CONNEXT_HAVE_TIME_UTILS */ duration->sec = static_cast(in_time.sec); duration->nanosec = static_cast(in_time.nsec); diff --git a/rmw_connextdds_common/src/common/rmw_impl_waitset_std.cpp b/rmw_connextdds_common/src/common/rmw_impl_waitset_std.cpp index 666d6e4e..cdfba613 100644 --- a/rmw_connextdds_common/src/common/rmw_impl_waitset_std.cpp +++ b/rmw_connextdds_common/src/common/rmw_impl_waitset_std.cpp @@ -631,24 +631,8 @@ RMW_Connext_SubscriberStatusCondition::has_status( { return this->triggered_qos; } -// Avoid warnings caused by RMW_EVENT_MESSAGE_LOST not being one of -// the defined values for rmw_event_type_t. This #if and the one in -// the `default` case, should be removed once support for releases -// without RMW_EVENT_MESSAGE_LOST is dropped (or the value is backported). -#if RMW_CONNEXT_HAVE_MESSAGE_LOST - case RMW_EVENT_MESSAGE_LOST: -#endif /* RMW_CONNEXT_HAVE_MESSAGE_LOST */ - { - return this->triggered_sample_lost; - } default: { -#if !RMW_CONNEXT_HAVE_MESSAGE_LOST - if (RMW_EVENT_MESSAGE_LOST == event_type) { - RMW_CONNEXT_LOG_WARNING( - "unexpected rmw_event_type_t: RMW_EVENT_MESSAGE_LOST") - } -#endif /* !RMW_CONNEXT_HAVE_MESSAGE_LOST */ RMW_CONNEXT_ASSERT(0) return false; } diff --git a/rmw_connextdds_common/src/common/rmw_node.cpp b/rmw_connextdds_common/src/common/rmw_node.cpp index 573a49b7..04270402 100644 --- a/rmw_connextdds_common/src/common/rmw_node.cpp +++ b/rmw_connextdds_common/src/common/rmw_node.cpp @@ -18,11 +18,6 @@ #include "rmw/validate_namespace.h" #include "rmw/validate_node_name.h" -#if RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_FOXY -// Required to look up LOCALHOST_ONLY env variable. -#include "rcutils/get_env.h" -#endif /* RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_FOXY */ - /****************************************************************************** * Node interface functions ******************************************************************************/ @@ -32,22 +27,12 @@ rmw_node_t * rmw_api_connextdds_create_node( rmw_context_t * context, const char * name, - const char * ns -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_DASHING - , - size_t domain_id, - const rmw_node_security_options_t * security_options -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - , + const char * ns, size_t domain_id, const rmw_node_security_options_t * security_options, - bool localhost_only -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_FOXY - , size_t domain_id, - bool localhost_only -#endif /* RMW_CONNEXT_RELEASE */ -) + const rmw_node_security_options_t * security_options, + bool localhost_only) { RMW_CHECK_ARGUMENT_FOR_NULL(context, nullptr); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( @@ -60,23 +45,11 @@ rmw_api_connextdds_create_node( "expected initialized context", return nullptr); - bool node_localhost_only = false; - -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_DASHING - UNUSED_ARG(security_options); -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT UNUSED_ARG(security_options); - node_localhost_only = localhost_only; -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_FOXY - node_localhost_only = localhost_only; -#else /* RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_FOXY */ - node_localhost_only = - context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED; -#endif /* RMW_CONNEXT_RELEASE > RMW_CONNEXT_RELEASE_FOXY */ RMW_CONNEXT_LOG_DEBUG_A( "creating new node: name=%s, ns=%s, localhost_only=%d", - name, ns, node_localhost_only) + name, ns, localhost_only) rmw_context_impl_t * ctx = context->impl; std::lock_guard guard(ctx->initialization_mutex); @@ -113,7 +86,6 @@ rmw_api_connextdds_create_node( RMW_CONNEXT_LOG_ERROR_A_SET("invalid node namespace: %s", reason) return nullptr; } -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_FOXY if (0u == ctx->node_count) { ctx->domain_id = domain_id; } else if ((size_t)ctx->domain_id != domain_id) { @@ -122,9 +94,8 @@ rmw_api_connextdds_create_node( ctx->domain_id, domain_id) return nullptr; } -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_FOXY */ - ret = ctx->initialize_node(ns, name, node_localhost_only); + ret = ctx->initialize_node(ns, name, localhost_only); if (RMW_RET_OK != ret) { RMW_CONNEXT_LOG_ERROR("failed to initialize node in context") return nullptr; @@ -271,12 +242,9 @@ rmw_api_connextdds_node_get_graph_guard_condition(const rmw_node_t * rmw_node) return node_impl->graph_guard_condition(); } -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - rmw_ret_t rmw_api_connextdds_node_assert_liveliness(const rmw_node_t * node) { UNUSED_ARG(node); return RMW_RET_UNSUPPORTED; } -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ diff --git a/rmw_connextdds_common/src/common/rmw_publication.cpp b/rmw_connextdds_common/src/common/rmw_publication.cpp index 5ab5aaec..19916f1c 100644 --- a/rmw_connextdds_common/src/common/rmw_publication.cpp +++ b/rmw_connextdds_common/src/common/rmw_publication.cpp @@ -93,11 +93,7 @@ rmw_api_connextdds_publish_loaned_message( rmw_ret_t rmw_api_connextdds_init_publisher_allocation( const rosidl_message_type_support_t * type_support, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT const rosidl_message_bounds_t * message_bounds, -#else - const rosidl_runtime_c__Sequence__bound * message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ rmw_publisher_allocation_t * allocation) { UNUSED_ARG(type_support); @@ -123,12 +119,8 @@ rmw_api_connextdds_create_publisher( const rmw_node_t * node, const rosidl_message_type_support_t * type_supports, const char * topic_name, - const rmw_qos_profile_t * qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , - const rmw_publisher_options_t * publisher_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -) + const rmw_qos_profile_t * qos_policies, + const rmw_publisher_options_t * publisher_options) { RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( @@ -139,9 +131,7 @@ rmw_api_connextdds_create_publisher( RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ RMW_CONNEXT_LOG_DEBUG_A( "creating new publisher: topic=%s", @@ -179,12 +169,8 @@ rmw_api_connextdds_create_publisher( ctx->dds_pub, type_supports, topic_name, - qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , - publisher_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - ); + qos_policies, + publisher_options); if (nullptr == rmw_pub) { RMW_CONNEXT_LOG_ERROR("failed to create RMW publisher") diff --git a/rmw_connextdds_common/src/common/rmw_qos.cpp b/rmw_connextdds_common/src/common/rmw_qos.cpp deleted file mode 100644 index 8fdd1ca3..00000000 --- a/rmw_connextdds_common/src/common/rmw_qos.cpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2020 Real-Time Innovations, Inc. (RTI) -// -// 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 "rmw_connextdds/rmw_impl.hpp" - -/****************************************************************************** - * QoS Profile functions - ******************************************************************************/ -#if RMW_CONNEXT_HAVE_QOS_PROFILE_API -rmw_ret_t -rmw_api_connextdds_qos_profile_check_compatible( - const rmw_qos_profile_t publisher_profile, - const rmw_qos_profile_t subscription_profile, - rmw_qos_compatibility_type_t * compatibility, - char * reason, - size_t reason_size) -{ - return rmw_dds_common::qos_profile_check_compatible( - publisher_profile, subscription_profile, compatibility, reason, reason_size); -} -#endif /* RMW_CONNEXT_HAVE_QOS_PROFILE_API */ diff --git a/rmw_connextdds_common/src/common/rmw_security_log.cpp b/rmw_connextdds_common/src/common/rmw_security_log.cpp deleted file mode 100644 index e002a68a..00000000 --- a/rmw_connextdds_common/src/common/rmw_security_log.cpp +++ /dev/null @@ -1,240 +0,0 @@ -// Copyright 2020 Canonical Ltd -// -// 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 "rmw_connextdds/rmw_impl.hpp" - -#if RMW_CONNEXT_HAVE_SECURITY - -#include "rcutils/get_env.h" - -// Connext 5.3.1 uses numeric levels (although note that v6 moved to using -// strings). These levels are: -// EMERGENCY: 0 -// ALERT: 1 -// CRITICAL: 2 -// ERROR: 3 -// WARNING: 4 -// NOTICE: 5 -// INFORMATIONAL: 6 -// DEBUG: 7 -// -// ROS has less logging levels, but it makes sense to use them here for -// consistency, so we have the following mapping. -const struct -{ - RCUTILS_LOG_SEVERITY ros_severity; - const char * const dds_level; -} RMW_Connext_fv_VerbosityMapping[] = -{ - {RCUTILS_LOG_SEVERITY_FATAL, "0"}, - {RCUTILS_LOG_SEVERITY_ERROR, "3"}, - {RCUTILS_LOG_SEVERITY_WARN, "4"}, - {RCUTILS_LOG_SEVERITY_INFO, "6"}, - {RCUTILS_LOG_SEVERITY_DEBUG, "7"}, -}; - -bool rmw_connextdds_severity_names_str(char buffer[], size_t buffer_size) -{ - size_t offset = 0; - - size_t severity_count = sizeof(RMW_Connext_fv_VerbosityMapping) / - sizeof(RMW_Connext_fv_VerbosityMapping[0]); - for (size_t i = 0; i < (severity_count - 1); ++i) { - RCUTILS_LOG_SEVERITY severity = - RMW_Connext_fv_VerbosityMapping[i].ros_severity; - - int length = rcutils_snprintf( - buffer + offset, buffer_size - offset, "%s, ", - g_rcutils_log_severity_names[severity]); - - if (length < 0) { - return false; - } - - offset += length; - if (offset >= buffer_size) { - return false; - } - } - - RCUTILS_LOG_SEVERITY severity = - RMW_Connext_fv_VerbosityMapping[severity_count - 1].ros_severity; - int length = rcutils_snprintf( - buffer + offset, buffer_size - offset, "or %s", - g_rcutils_log_severity_names[severity]); - - if (length < 0) { - return false; - } - - return (offset + length) <= buffer_size; -} - -bool -rmw_connextdds_string_to_verbosity_level( - const char * const str, const char ** level) -{ - int ros_severity; - if (rcutils_logging_severity_level_from_string( - str, - rcutils_get_default_allocator(), &ros_severity) == RCUTILS_RET_OK) - { - for (const auto & item : RMW_Connext_fv_VerbosityMapping) { - if (ros_severity == item.ros_severity) { - *level = item.dds_level; - return true; - } - } - } - - return false; -} - -bool -rmw_connextdds_validate_boolean(const char * const val) -{ - return nullptr != val && - (strcmp(val, "true") == 0 || strcmp(val, "false") == 0); -} - -rmw_ret_t -rmw_connextdds_apply_security_logging_configuration( - DDS_PropertyQosPolicy * const properties) -{ - // Check if secure logs should be saved to a local file. - const char * env_val = nullptr; - const char * lookup_rc = - rcutils_get_env(RMW_CONNEXT_ENV_SECURITY_LOG_FILE, &env_val); - - if (nullptr != lookup_rc || nullptr == env_val) { - RMW_CONNEXT_LOG_ERROR_A_SET( - "failed to lookup from environment: " - "var=%s, " - "rc=%s ", - RMW_CONNEXT_ENV_SECURITY_LOG_FILE, - lookup_rc) - return RMW_RET_ERROR; - } - - if ('\0' != env_val[0]) { - if (DDS_RETCODE_OK != - DDS_PropertyQosPolicyHelper_assert_property( - properties, - DDS_SECURITY_LOGGING_FILE_PROPERTY, - env_val, - RTI_FALSE)) - { - RMW_CONNEXT_LOG_ERROR_A_SET( - "failed to assert DDS property: '%s' = '%s'", - DDS_SECURITY_LOGGING_FILE_PROPERTY, env_val) - return RMW_RET_ERROR; - } - } - - // Check if secure logs should be distributed over DDS - env_val = nullptr; - lookup_rc = - rcutils_get_env(RMW_CONNEXT_ENV_SECURITY_LOG_PUBLISH, &env_val); - - if (nullptr != lookup_rc || nullptr == env_val) { - RMW_CONNEXT_LOG_ERROR_A_SET( - "failed to lookup from environment: " - "var=%s, " - "rc=%s ", - RMW_CONNEXT_ENV_SECURITY_LOG_PUBLISH, - lookup_rc) - return RMW_RET_ERROR; - } - - if ('\0' != env_val[0]) { - if (!rmw_connextdds_validate_boolean(env_val)) { - RMW_CONNEXT_LOG_ERROR_A_SET( - "invalid value for %s: '%s' (use 'true' or 'false')", - RMW_CONNEXT_ENV_SECURITY_LOG_PUBLISH, - env_val) - return RMW_RET_ERROR; - } - - if (DDS_RETCODE_OK != - DDS_PropertyQosPolicyHelper_assert_property( - properties, - DDS_SECURITY_LOGGING_DISTRIBUTE_PROPERTY, - env_val, - RTI_FALSE)) - { - RMW_CONNEXT_LOG_ERROR_A_SET( - "failed to assert DDS property: '%s' = '%s'", - DDS_SECURITY_LOGGING_DISTRIBUTE_PROPERTY, env_val) - return RMW_RET_ERROR; - } - } - - // Check verbosity level for secure logs. - env_val = nullptr; - lookup_rc = - rcutils_get_env(RMW_CONNEXT_ENV_SECURITY_LOG_VERBOSITY, &env_val); - - if (nullptr != lookup_rc || nullptr == env_val) { - RMW_CONNEXT_LOG_ERROR_A_SET( - "failed to lookup from environment: " - "var=%s, " - "rc=%s ", - RMW_CONNEXT_ENV_SECURITY_LOG_VERBOSITY, - lookup_rc) - return RMW_RET_ERROR; - } - - if ('\0' != env_val[0]) { - const char * level = nullptr; - if (!rmw_connextdds_string_to_verbosity_level(env_val, &level)) { - char humanized_severity_list[RCUTILS_ERROR_MESSAGE_MAX_LENGTH]; - if (!rmw_connextdds_severity_names_str( - humanized_severity_list, - RCUTILS_ERROR_MESSAGE_MAX_LENGTH)) - { - RMW_CONNEXT_LOG_ERROR_SET("unable to create severity string") - humanized_severity_list[0] = '\0'; - } - RMW_CONNEXT_LOG_ERROR_A_SET( - "invalid value for %s: '%s' (use one of: %s)", - RMW_CONNEXT_ENV_SECURITY_LOG_VERBOSITY, - env_val, - humanized_severity_list); - return RMW_RET_ERROR; - } - if (nullptr == level) { - RMW_CONNEXT_LOG_ERROR_A_SET( - "failed to convert value for %s: '%s'", - RMW_CONNEXT_ENV_SECURITY_LOG_VERBOSITY, - env_val) - return RMW_RET_ERROR; - } - - if (DDS_RETCODE_OK != - DDS_PropertyQosPolicyHelper_assert_property( - properties, - DDS_SECURITY_LOGGING_LEVEL_PROPERTY, - level, - RTI_FALSE)) - { - RMW_CONNEXT_LOG_ERROR_A_SET( - "failed to assert DDS property: '%s' = '%s'", - DDS_SECURITY_LOGGING_LEVEL_PROPERTY, level) - return RMW_RET_ERROR; - } - } - - return RMW_RET_OK; -} -#endif /* RMW_CONNEXT_HAVE_SECURITY */ diff --git a/rmw_connextdds_common/src/common/rmw_serde.cpp b/rmw_connextdds_common/src/common/rmw_serde.cpp index 3248d53a..823fa4e6 100644 --- a/rmw_connextdds_common/src/common/rmw_serde.cpp +++ b/rmw_connextdds_common/src/common/rmw_serde.cpp @@ -22,11 +22,7 @@ rmw_ret_t rmw_api_connextdds_get_serialized_message_size( const rosidl_message_type_support_t * type_supports, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT const rosidl_message_bounds_t * message_bounds, -#else - const rosidl_runtime_c__Sequence__bound * message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ size_t * size) { UNUSED_ARG(type_supports); @@ -46,9 +42,6 @@ rmw_api_connextdds_serialize( try { // declare a mock context struct to build a temporary type support rmw_context_t ctx_base; -#if RMW_CONNEXT_HAVE_OPTIONS - ctx_base.options.localhost_only = RMW_LOCALHOST_ONLY_DISABLED; -#endif /* RMW_CONNEXT_HAVE_OPTIONS */ rmw_context_impl_t ctx(&ctx_base); ctx.request_reply_mapping = RMW_Connext_RequestReplyMapping::Extended; RMW_Connext_MessageTypeSupport type_support( @@ -85,9 +78,6 @@ rmw_api_connextdds_deserialize( try { // declare a mock context struct to build a temporary type support rmw_context_t ctx_base; -#if RMW_CONNEXT_HAVE_OPTIONS - ctx_base.options.localhost_only = RMW_LOCALHOST_ONLY_DISABLED; -#endif /* RMW_CONNEXT_HAVE_OPTIONS */ rmw_context_impl_t ctx(&ctx_base); ctx.request_reply_mapping = RMW_Connext_RequestReplyMapping::Extended; RMW_Connext_MessageTypeSupport type_support( diff --git a/rmw_connextdds_common/src/common/rmw_service.cpp b/rmw_connextdds_common/src/common/rmw_service.cpp index 981e2e96..18efbab0 100644 --- a/rmw_connextdds_common/src/common/rmw_service.cpp +++ b/rmw_connextdds_common/src/common/rmw_service.cpp @@ -25,11 +25,7 @@ rmw_ret_t rmw_api_connextdds_take_response( const rmw_client_t * client, -#if RMW_CONNEXT_HAVE_SERVICE_INFO - rmw_service_info_t * request_header, -#else rmw_request_id_t * request_header, -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ void * ros_response, bool * taken) { @@ -46,9 +42,6 @@ rmw_api_connextdds_take_response( RMW_Connext_Client * const client_impl = reinterpret_cast(client->data); -#if RMW_CONNEXT_HAVE_SERVICE_INFO - return client_impl->take_response(request_header, ros_response, taken); -#else rmw_service_info_t request_header_s; rmw_ret_t rc = client_impl->take_response(&request_header_s, ros_response, taken); @@ -57,18 +50,13 @@ rmw_api_connextdds_take_response( } *request_header = request_header_s.request_id; return RMW_RET_OK; -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ } rmw_ret_t rmw_api_connextdds_take_request( const rmw_service_t * service, -#if RMW_CONNEXT_HAVE_SERVICE_INFO - rmw_service_info_t * request_header, -#else rmw_request_id_t * request_header, -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ void * ros_request, bool * taken) { @@ -85,9 +73,6 @@ rmw_api_connextdds_take_request( RMW_Connext_Service * const svc_impl = reinterpret_cast(service->data); -#if RMW_CONNEXT_HAVE_SERVICE_INFO - return svc_impl->take_request(request_header, ros_request, taken); -#else rmw_service_info_t request_header_s; rmw_ret_t rc = svc_impl->take_request(&request_header_s, ros_request, taken); @@ -96,7 +81,6 @@ rmw_api_connextdds_take_request( } *request_header = request_header_s.request_id; return RMW_RET_OK; -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ } diff --git a/rmw_connextdds_common/src/common/rmw_subscription.cpp b/rmw_connextdds_common/src/common/rmw_subscription.cpp index 622203e6..e3b7e14c 100644 --- a/rmw_connextdds_common/src/common/rmw_subscription.cpp +++ b/rmw_connextdds_common/src/common/rmw_subscription.cpp @@ -25,11 +25,7 @@ rmw_ret_t rmw_api_connextdds_init_subscription_allocation( const rosidl_message_type_support_t * type_support, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT const rosidl_message_bounds_t * message_bounds, -#else - const rosidl_runtime_c__Sequence__bound * message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ rmw_subscription_allocation_t * allocation) { UNUSED_ARG(type_support); @@ -56,12 +52,7 @@ rmw_api_connextdds_create_subscription( const rosidl_message_type_support_t * type_supports, const char * topic_name, const rmw_qos_profile_t * qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - const rmw_subscription_options_t * subscription_options -#else - bool ignore_local_publications -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -) + const rmw_subscription_options_t * subscription_options) { RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( @@ -72,9 +63,7 @@ rmw_api_connextdds_create_subscription( RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ RMW_CONNEXT_LOG_DEBUG_A( "creating new subscription: topic=%s", @@ -113,12 +102,7 @@ rmw_api_connextdds_create_subscription( type_supports, topic_name, qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - subscription_options -#else - ignore_local_publications -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - ); + subscription_options); if (nullptr == rmw_sub) { RMW_CONNEXT_LOG_ERROR("failed to create RMW subscription") @@ -265,53 +249,6 @@ rmw_api_connextdds_take_with_info( return rc; } -#if RMW_CONNEXT_HAVE_TAKE_SEQ - - -rmw_ret_t -rmw_api_connextdds_take_sequence( - const rmw_subscription_t * subscription, - size_t count, - rmw_message_sequence_t * message_sequence, - rmw_message_info_sequence_t * message_info_sequence, - size_t * taken, - rmw_subscription_allocation_t * allocation) -{ - RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - subscription, - subscription->implementation_identifier, - RMW_CONNEXTDDS_ID, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - RMW_CHECK_ARGUMENT_FOR_NULL(message_sequence, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL( - message_info_sequence, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); - - UNUSED_ARG(allocation); - - RMW_Connext_Subscriber * const sub_impl = - reinterpret_cast(subscription->data); - - // Reset length of output sequences - message_sequence->size = 0; - message_info_sequence->size = 0; - - rmw_ret_t rc = sub_impl->take( - message_sequence, message_info_sequence, count, taken); - - // Update length of output sequences if we received any data - if (*taken > 0) { - message_sequence->size = *taken; - message_info_sequence->size = *taken; - } - - return rc; -} - -#endif /* RMW_CONNEXT_HAVE_TAKE_SEQ */ - - rmw_ret_t rmw_api_connextdds_take_serialized_message( const rmw_subscription_t * subscription, diff --git a/rmw_connextdds_common/src/ndds/dds_api_ndds.cpp b/rmw_connextdds_common/src/ndds/dds_api_ndds.cpp index 1fc9fe67..78eb88bf 100644 --- a/rmw_connextdds_common/src/ndds/dds_api_ndds.cpp +++ b/rmw_connextdds_common/src/ndds/dds_api_ndds.cpp @@ -124,46 +124,6 @@ rmw_connextdds_initialize_participant_qos_impl( } #endif /* RMW_CONNEXT_DONT_IGNORE_LOOPBACK_INTERFACE */ -#if RMW_CONNEXT_HAVE_OPTIONS - const size_t user_data_len_in = - DDS_OctetSeq_get_length(&dp_qos->user_data.value); - - if (user_data_len_in != 0) { - RMW_CONNEXT_LOG_WARNING( - "DomainParticipant's USER_DATA will be overwritten to " - "propagate node enclave") - } - - const char * const user_data_fmt = "enclave=%s;"; - - const int user_data_len = - std::snprintf( - nullptr, 0, user_data_fmt, ctx->base->options.enclave) + 1; - - if (!DDS_OctetSeq_ensure_length( - &dp_qos->user_data.value, user_data_len, user_data_len)) - { - RMW_CONNEXT_LOG_ERROR_SET("failed to set user_data length") - return RMW_RET_ERROR; - } - - char * const user_data_ptr = - reinterpret_cast( - DDS_OctetSeq_get_contiguous_buffer(&dp_qos->user_data.value)); - - const int user_data_rc = - std::snprintf( - user_data_ptr, - user_data_len, - user_data_fmt, - ctx->base->options.enclave); - - if (user_data_rc < 0 || user_data_rc != user_data_len - 1) { - RMW_CONNEXT_LOG_ERROR_SET("failed to set user_data") - return RMW_RET_ERROR; - } -#endif /* RMW_CONNEXT_HAVE_OPTIONS */ - #if RMW_CONNEXT_RTPS_AUTO_ID_FROM_UUID dp_qos->wire_protocol.rtps_auto_id_kind = DDS_RTPS_AUTO_ID_FROM_UUID; #endif /* RMW_CONNEXT_RTPS_AUTO_ID_FROM_UUID */ @@ -272,19 +232,13 @@ rmw_connextdds_get_qos_policies( const bool writer_qos, RMW_Connext_MessageTypeSupport * const type_support, DDS_PropertyQosPolicy * const property, - const rmw_qos_profile_t * const qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , + const rmw_qos_profile_t * const qos_policies, const rmw_publisher_options_t * const pub_options, - const rmw_subscription_options_t * const sub_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -) + const rmw_subscription_options_t * const sub_options) { UNUSED_ARG(qos_policies); -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB UNUSED_ARG(pub_options); UNUSED_ARG(sub_options); -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ /* if type is unbound set property to force allocation of samples from heap */ if (type_support->unbounded()) { @@ -315,12 +269,8 @@ rmw_connextdds_get_datawriter_qos( RMW_Connext_MessageTypeSupport * const type_support, DDS_Topic * const topic, DDS_DataWriterQos * const qos, - const rmw_qos_profile_t * const qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , - const rmw_publisher_options_t * const pub_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -) + const rmw_qos_profile_t * const qos_policies, + const rmw_publisher_options_t * const pub_options) { UNUSED_ARG(topic); @@ -346,16 +296,9 @@ rmw_connextdds_get_datawriter_qos( &qos->resource_limits, // TODO(asorbini) this value is not actually used, remove it &qos->publish_mode, - #if RMW_CONNEXT_HAVE_LIFESPAN_QOS - &qos->lifespan, - #endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ - qos_policies - #if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , + qos_policies, pub_options, - nullptr /* sub_options */ - #endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - )) + nullptr /* sub_options */)) { return RMW_RET_ERROR; } @@ -403,13 +346,9 @@ rmw_connextdds_get_datawriter_qos( true /* writer_qos */, type_support, &qos->property, - qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , + qos_policies, pub_options, - nullptr /* sub_options */ -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - ); + nullptr /* sub_options */); } rmw_ret_t @@ -418,11 +357,8 @@ rmw_connextdds_get_datareader_qos( RMW_Connext_MessageTypeSupport * const type_support, DDS_TopicDescription * const topic_desc, DDS_DataReaderQos * const qos, - const rmw_qos_profile_t * const qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , const rmw_subscription_options_t * const sub_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -) + const rmw_qos_profile_t * const qos_policies, + const rmw_subscription_options_t * const sub_options) { UNUSED_ARG(ctx); UNUSED_ARG(topic_desc); @@ -448,16 +384,9 @@ rmw_connextdds_get_datareader_qos( &qos->liveliness, &qos->resource_limits, nullptr /* publish_mode */, - #if RMW_CONNEXT_HAVE_LIFESPAN_QOS - nullptr /* Lifespan is a writer-only qos policy */, - #endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ - qos_policies - #if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , + qos_policies, nullptr /* pub_options */, - sub_options - #endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - )) + sub_options)) { return RMW_RET_ERROR; } @@ -484,13 +413,9 @@ rmw_connextdds_get_datareader_qos( false /* writer_qos */, type_support, &qos->property, - qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , + qos_policies, nullptr /* pub_options */, - sub_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - ); + sub_options); } DDS_DataWriter * @@ -499,9 +424,7 @@ rmw_connextdds_create_datawriter( DDS_DomainParticipant * const participant, DDS_Publisher * const pub, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_publisher_options_t * const publisher_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal, RMW_Connext_MessageTypeSupport * const type_support, DDS_Topic * const topic, @@ -513,11 +436,7 @@ rmw_connextdds_create_datawriter( if (RMW_RET_OK != rmw_connextdds_get_datawriter_qos( - ctx, type_support, topic, dw_qos, qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , publisher_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - )) + ctx, type_support, topic, dw_qos, qos_policies, publisher_options)) { RMW_CONNEXT_LOG_ERROR("failed to convert writer QoS") return nullptr; @@ -537,9 +456,7 @@ rmw_connextdds_create_datareader( DDS_DomainParticipant * const participant, DDS_Subscriber * const sub, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_subscription_options_t * const subscriber_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal, RMW_Connext_MessageTypeSupport * const type_support, DDS_TopicDescription * const topic_desc, @@ -551,11 +468,7 @@ rmw_connextdds_create_datareader( if (RMW_RET_OK != rmw_connextdds_get_datareader_qos( - ctx, type_support, topic_desc, dr_qos, qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , subscriber_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - )) + ctx, type_support, topic_desc, dr_qos, qos_policies, subscriber_options)) { RMW_CONNEXT_LOG_ERROR("failed to convert reader QoS") return nullptr; @@ -1102,9 +1015,6 @@ rmw_connextdds_dcps_publication_on_data(rmw_context_impl_t * const ctx) &data->durability, &data->deadline, &data->liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - &data->lifespan, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ false /* is_reader */); } @@ -1187,9 +1097,6 @@ rmw_connextdds_dcps_subscription_on_data(rmw_context_impl_t * const ctx) &data->durability, &data->deadline, &data->liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - nullptr /* Lifespan is a writer-only qos policy */, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ true /* is_reader */); } diff --git a/rmw_connextdds_common/src/rmw_dds_common/topic_endpoint_info.c b/rmw_connextdds_common/src/rmw_dds_common/topic_endpoint_info.c deleted file mode 100644 index af1e0df5..00000000 --- a/rmw_connextdds_common/src/rmw_dds_common/topic_endpoint_info.c +++ /dev/null @@ -1,228 +0,0 @@ -// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. -// -// 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 "rmw_connextdds/static_config.hpp" - -#if !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON - -#include "rmw_connextdds/topic_endpoint_info.h" - -#include "rcutils/strdup.h" -#include "rmw/error_handling.h" -#include "rmw/types.h" - -rmw_topic_endpoint_info_t -rmw_get_zero_initialized_topic_endpoint_info(void) -{ -#ifdef __clang__ -# pragma clang diagnostic push -# pragma clang diagnostic ignored "-Wmissing-field-initializers" -#endif - rmw_topic_endpoint_info_t zero = {0}; -#ifdef __clang__ -# pragma clang diagnostic pop -#endif - return zero; -} - -rmw_ret_t -_rmw_topic_endpoint_info_fini_str( - const char ** topic_endpoint_info_str, - rcutils_allocator_t * allocator) -{ - allocator->deallocate((char *) *topic_endpoint_info_str, allocator->state); - *topic_endpoint_info_str = NULL; - return RMW_RET_OK; -} - -rmw_ret_t -_rmw_topic_endpoint_info_fini_node_name( - rmw_topic_endpoint_info_t * topic_endpoint_info, - rcutils_allocator_t * allocator) -{ - return _rmw_topic_endpoint_info_fini_str(&topic_endpoint_info->node_name, allocator); -} - -rmw_ret_t -_rmw_topic_endpoint_info_fini_node_namespace( - rmw_topic_endpoint_info_t * topic_endpoint_info, - rcutils_allocator_t * allocator) -{ - return _rmw_topic_endpoint_info_fini_str(&topic_endpoint_info->node_namespace, allocator); -} - -rmw_ret_t -_rmw_topic_endpoint_info_fini_topic_type( - rmw_topic_endpoint_info_t * topic_endpoint_info, - rcutils_allocator_t * allocator) -{ - return _rmw_topic_endpoint_info_fini_str(&topic_endpoint_info->topic_type, allocator); -} - -rmw_ret_t -rmw_topic_endpoint_info_fini( - rmw_topic_endpoint_info_t * topic_endpoint_info, - rcutils_allocator_t * allocator) -{ - if (!topic_endpoint_info) { - RMW_SET_ERROR_MSG("topic_endpoint_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - if (!allocator) { - RMW_SET_ERROR_MSG("allocator is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - rmw_ret_t ret; - ret = _rmw_topic_endpoint_info_fini_node_name(topic_endpoint_info, allocator); - if (ret != RMW_RET_OK) { - return ret; - } - ret = _rmw_topic_endpoint_info_fini_node_namespace(topic_endpoint_info, allocator); - if (ret != RMW_RET_OK) { - return ret; - } - ret = _rmw_topic_endpoint_info_fini_topic_type(topic_endpoint_info, allocator); - if (ret != RMW_RET_OK) { - return ret; - } - - *topic_endpoint_info = rmw_get_zero_initialized_topic_endpoint_info(); - - return RMW_RET_OK; -} - -rmw_ret_t -_rmw_topic_endpoint_info_copy_str( - const char ** topic_endpoint_info_str, - const char * str, - rcutils_allocator_t * allocator) -{ - if (!str) { - RMW_SET_ERROR_MSG("str is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - if (!topic_endpoint_info_str) { - RMW_SET_ERROR_MSG("topic_endpoint_info_str is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - if (!allocator) { - RMW_SET_ERROR_MSG("allocator is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - *topic_endpoint_info_str = rcutils_strdup(str, *allocator); - - return RMW_RET_OK; -} - -rmw_ret_t -rmw_topic_endpoint_info_set_topic_type( - rmw_topic_endpoint_info_t * topic_endpoint_info, - const char * topic_type, - rcutils_allocator_t * allocator) -{ - if (!topic_endpoint_info) { - RMW_SET_ERROR_MSG("topic_endpoint_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_endpoint_info_copy_str(&topic_endpoint_info->topic_type, topic_type, allocator); -} - -rmw_ret_t -rmw_topic_endpoint_info_set_node_name( - rmw_topic_endpoint_info_t * topic_endpoint_info, - const char * node_name, - rcutils_allocator_t * allocator) -{ - if (!topic_endpoint_info) { - RMW_SET_ERROR_MSG("topic_endpoint_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_endpoint_info_copy_str(&topic_endpoint_info->node_name, node_name, allocator); -} - -rmw_ret_t -rmw_topic_endpoint_info_set_node_namespace( - rmw_topic_endpoint_info_t * topic_endpoint_info, - const char * node_namespace, - rcutils_allocator_t * allocator) -{ - if (!topic_endpoint_info) { - RMW_SET_ERROR_MSG("topic_endpoint_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - return _rmw_topic_endpoint_info_copy_str( - &topic_endpoint_info->node_namespace, - node_namespace, - allocator); -} - -rmw_ret_t -rmw_topic_endpoint_info_set_endpoint_type( - rmw_topic_endpoint_info_t * topic_endpoint_info, - rmw_endpoint_type_t type) -{ - if (!topic_endpoint_info) { - RMW_SET_ERROR_MSG("topic_endpoint_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - topic_endpoint_info->endpoint_type = type; - - return RMW_RET_OK; -} - -rmw_ret_t -rmw_topic_endpoint_info_set_gid( - rmw_topic_endpoint_info_t * topic_endpoint_info, - const uint8_t gid[], - size_t size) -{ - if (!topic_endpoint_info) { - RMW_SET_ERROR_MSG("topic_endpoint_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - if (size > RMW_GID_STORAGE_SIZE) { - RMW_SET_ERROR_MSG("size is more than RMW_GID_STORAGE_SIZE"); - return RMW_RET_INVALID_ARGUMENT; - } - memset(topic_endpoint_info->endpoint_gid, 0, RMW_GID_STORAGE_SIZE); - memcpy(topic_endpoint_info->endpoint_gid, gid, size); - return RMW_RET_OK; -} - -rmw_ret_t -rmw_topic_endpoint_info_set_qos_profile( - rmw_topic_endpoint_info_t * topic_endpoint_info, - const rmw_qos_profile_t * qos_profile) -{ - if (!topic_endpoint_info) { - RMW_SET_ERROR_MSG("topic_endpoint_info is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - if (!qos_profile) { - RMW_SET_ERROR_MSG("qos_profile is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - topic_endpoint_info->qos_profile = *qos_profile; - return RMW_RET_OK; -} - - -#endif /* !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON */ diff --git a/rmw_connextdds_common/src/rmw_dds_common/topic_endpoint_info_array.c b/rmw_connextdds_common/src/rmw_dds_common/topic_endpoint_info_array.c deleted file mode 100644 index 355635cc..00000000 --- a/rmw_connextdds_common/src/rmw_dds_common/topic_endpoint_info_array.c +++ /dev/null @@ -1,110 +0,0 @@ -// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. -// -// 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 "rmw_connextdds/static_config.hpp" - -#if !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON - -#include "rmw_connextdds/topic_endpoint_info_array.h" -#include "rmw/error_handling.h" -#include "rmw/types.h" - -rmw_topic_endpoint_info_array_t -rmw_get_zero_initialized_topic_endpoint_info_array(void) -{ -#ifdef __clang__ -# pragma clang diagnostic push -# pragma clang diagnostic ignored "-Wmissing-field-initializers" -#endif - rmw_topic_endpoint_info_array_t zero = {0}; -#ifdef __clang__ -# pragma clang diagnostic pop -#endif - return zero; -} - -rmw_ret_t -rmw_topic_endpoint_info_array_check_zero( - rmw_topic_endpoint_info_array_t * topic_endpoint_info_array) -{ - if (!topic_endpoint_info_array) { - RMW_SET_ERROR_MSG("topic_endpoint_info_array is null"); - return RMW_RET_INVALID_ARGUMENT; - } - if (topic_endpoint_info_array->size != 0 || topic_endpoint_info_array->info_array != NULL) { - RMW_SET_ERROR_MSG("topic_endpoint_info_array is not zeroed"); - return RMW_RET_ERROR; - } - return RMW_RET_OK; -} - -rmw_ret_t -rmw_topic_endpoint_info_array_init_with_size( - rmw_topic_endpoint_info_array_t * topic_endpoint_info_array, - size_t size, - rcutils_allocator_t * allocator) -{ - if (!allocator) { - RMW_SET_ERROR_MSG("allocator is null"); - return RMW_RET_INVALID_ARGUMENT; - } - if (!topic_endpoint_info_array) { - RMW_SET_ERROR_MSG("topic_endpoint_info_array is null"); - return RMW_RET_INVALID_ARGUMENT; - } - topic_endpoint_info_array->info_array = - allocator->allocate(sizeof(*topic_endpoint_info_array->info_array) * size, allocator->state); - if (!topic_endpoint_info_array->info_array) { - RMW_SET_ERROR_MSG("failed to allocate memory for info_array"); - return RMW_RET_BAD_ALLOC; - } - topic_endpoint_info_array->size = size; - for (size_t i = 0; i < size; i++) { - topic_endpoint_info_array->info_array[i] = rmw_get_zero_initialized_topic_endpoint_info(); - } - return RMW_RET_OK; -} - -rmw_ret_t -rmw_topic_endpoint_info_array_fini( - rmw_topic_endpoint_info_array_t * topic_endpoint_info_array, - rcutils_allocator_t * allocator) -{ - if (!allocator) { - RMW_SET_ERROR_MSG("allocator is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - if (!topic_endpoint_info_array) { - RMW_SET_ERROR_MSG("topic_endpoint_info_array is null"); - return RMW_RET_INVALID_ARGUMENT; - } - - rmw_ret_t ret; - // free all const char * inside the topic_endpoint_info_t - for (size_t i = 0u; i < topic_endpoint_info_array->size; i++) { - ret = rmw_topic_endpoint_info_fini(&topic_endpoint_info_array->info_array[i], allocator); - if (ret != RMW_RET_OK) { - return ret; - } - } - - allocator->deallocate(topic_endpoint_info_array->info_array, allocator->state); - topic_endpoint_info_array->info_array = NULL; - topic_endpoint_info_array->size = 0; - return RMW_RET_OK; -} - -#endif /* !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON */ diff --git a/rmw_connextdds_common/src/rtime/dds_api_rtime.cpp b/rmw_connextdds_common/src/rtime/dds_api_rtime.cpp index 0c46ea9d..3f674188 100644 --- a/rmw_connextdds_common/src/rtime/dds_api_rtime.cpp +++ b/rmw_connextdds_common/src/rtime/dds_api_rtime.cpp @@ -905,13 +905,9 @@ rmw_connextdds_get_qos_policies( DDS_DataWriterResourceLimitsQosPolicy * const writer_resource_limits, DDS_DataReaderProtocolQosPolicy * const reader_protocol, DDS_DataWriterProtocolQosPolicy * const writer_protocol, - const rmw_qos_profile_t * const qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , + const rmw_qos_profile_t * const qos_policies, const rmw_publisher_options_t * const pub_options, - const rmw_subscription_options_t * const sub_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -) + const rmw_subscription_options_t * const sub_options) { UNUSED_ARG(type_support); UNUSED_ARG(writer_qos); @@ -920,10 +916,8 @@ rmw_connextdds_get_qos_policies( UNUSED_ARG(deadline); UNUSED_ARG(liveliness); UNUSED_ARG(qos_policies); -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB UNUSED_ARG(pub_options); UNUSED_ARG(sub_options); -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ /* Adjust resource limits according to other QoS policies */ size_t max_samples = RMW_CONNEXT_LIMIT_SAMPLES_MAX; @@ -988,12 +982,8 @@ rmw_connextdds_get_datawriter_qos( RMW_Connext_MessageTypeSupport * const type_support, DDS_Topic * const topic, DDS_DataWriterQos * const qos, - const rmw_qos_profile_t * const qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , - const rmw_publisher_options_t * const pub_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -) + const rmw_qos_profile_t * const qos_policies, + const rmw_publisher_options_t * const pub_options) { UNUSED_ARG(ctx); UNUSED_ARG(topic); @@ -1009,16 +999,9 @@ rmw_connextdds_get_datawriter_qos( &qos->liveliness, &qos->resource_limits, &qos->publish_mode, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - nullptr /* Micro doesn't support DDS_LifespanQosPolicy */, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ - qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , + qos_policies, pub_options, - nullptr /* sub_options */ -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - )) + nullptr /* sub_options */)) { return RMW_RET_ERROR; } @@ -1036,13 +1019,9 @@ rmw_connextdds_get_datawriter_qos( &qos->writer_resource_limits, nullptr /* reader protocol */, &qos->protocol, - qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , + qos_policies, pub_options, - nullptr /* sub_options */ -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - ); + nullptr /* sub_options */); } rmw_ret_t @@ -1051,11 +1030,8 @@ rmw_connextdds_get_datareader_qos( RMW_Connext_MessageTypeSupport * const type_support, DDS_TopicDescription * const topic_desc, DDS_DataReaderQos * const qos, - const rmw_qos_profile_t * const qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , const rmw_subscription_options_t * const sub_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -) + const rmw_qos_profile_t * const qos_policies, + const rmw_subscription_options_t * const sub_options) { UNUSED_ARG(ctx); UNUSED_ARG(topic_desc); @@ -1071,16 +1047,9 @@ rmw_connextdds_get_datareader_qos( &qos->liveliness, &qos->resource_limits, nullptr /* publish_mode */, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - nullptr /* Lifespan is a writer-only qos policy */, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ - qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , + qos_policies, nullptr /* pub_options */, - sub_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - )) + sub_options)) { return RMW_RET_ERROR; } @@ -1097,13 +1066,9 @@ rmw_connextdds_get_datareader_qos( nullptr /* writer_resource_limits */, &qos->protocol, nullptr /* writer protocol */, - qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , + qos_policies, nullptr /* pub_options */, - sub_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - ); + sub_options); } DDS_DataWriter * @@ -1112,9 +1077,7 @@ rmw_connextdds_create_datawriter( DDS_DomainParticipant * const participant, DDS_Publisher * const pub, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_publisher_options_t * const publisher_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal, RMW_Connext_MessageTypeSupport * const type_support, DDS_Topic * const topic, @@ -1126,11 +1089,7 @@ rmw_connextdds_create_datawriter( if (RMW_RET_OK != rmw_connextdds_get_datawriter_qos( - ctx, type_support, topic, dw_qos, qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , publisher_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - )) + ctx, type_support, topic, dw_qos, qos_policies, publisher_options)) { RMW_CONNEXT_LOG_ERROR("failed to convert writer QoS") return nullptr; @@ -1149,9 +1108,7 @@ rmw_connextdds_create_datareader( DDS_DomainParticipant * const participant, DDS_Subscriber * const sub, const rmw_qos_profile_t * const qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB const rmw_subscription_options_t * const subscriber_options, -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ const bool internal, RMW_Connext_MessageTypeSupport * const type_support, DDS_TopicDescription * const topic_desc, @@ -1163,11 +1120,7 @@ rmw_connextdds_create_datareader( if (RMW_RET_OK != rmw_connextdds_get_datareader_qos( - ctx, type_support, topic_desc, dr_qos, qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , subscriber_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - )) + ctx, type_support, topic_desc, dr_qos, qos_policies, subscriber_options)) { RMW_CONNEXT_LOG_ERROR("failed to convert reader QoS") return nullptr; @@ -1800,9 +1753,6 @@ rmw_connextdds_dcps_publication_on_data(rmw_context_impl_t * const ctx) &qdata.data.durability, &qdata.data.deadline, &qdata.data.liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - nullptr /* Micro doesn't support DDS_LifespanQosPolicy */, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ false /* is_reader */); RMW_Connext_PublicationData::finalize(&qdata.data); @@ -1860,9 +1810,6 @@ rmw_connextdds_dcps_subscription_on_data(rmw_context_impl_t * const ctx) &qdata.data.durability, &qdata.data.deadline, &qdata.data.liveliness, -#if RMW_CONNEXT_HAVE_LIFESPAN_QOS - nullptr /* Lifespan is a writer-only qos policy */, -#endif /* RMW_CONNEXT_HAVE_LIFESPAN_QOS */ true /* is_reader */); RMW_Connext_SubscriptionData::finalize(&qdata.data); diff --git a/rmw_connextddsmicro/CMakeLists.txt b/rmw_connextddsmicro/CMakeLists.txt index b8c841ad..91ff6bcc 100644 --- a/rmw_connextddsmicro/CMakeLists.txt +++ b/rmw_connextddsmicro/CMakeLists.txt @@ -53,13 +53,7 @@ target_include_directories(${PROJECT_NAME} PRIVATE target_link_libraries(${PROJECT_NAME} rmw_connextdds_common::rmw_connextdds_common_micro) -# Unless we need to link the rmw_dds_common shim library generated -# by rmw_connextdds_common (produced if RMW_CONNEXT_PROVIDE_RMW_DDS_COMMON) -# we don't need to call ament_target_dependencies() since we are already -# linking library rmw_connextdds_common_micro above. -if(RMW_CONNEXT_PROVIDE_RMW_DDS_COMMON) - ament_target_dependencies(${PROJECT_NAME} rmw_connextdds_common) -endif() +ament_target_dependencies(${PROJECT_NAME} rmw_connextdds_common) configure_rmw_library(${PROJECT_NAME}) diff --git a/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp b/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp index 9b22efca..4483145b 100644 --- a/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp +++ b/rmw_connextddsmicro/src/rmw_api_impl_rtime.cpp @@ -287,40 +287,13 @@ rmw_node_t * rmw_create_node( rmw_context_t * context, const char * name, - const char * ns -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_DASHING - , - size_t domain_id, - const rmw_node_security_options_t * security_options -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - , + const char * ns, size_t domain_id, const rmw_node_security_options_t * security_options, - bool localhost_only -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_FOXY - , - size_t domain_id, - bool localhost_only -#endif /* RMW_CONNEXT_RELEASE */ -) + bool localhost_only) { return rmw_api_connextdds_create_node( - context, name, ns -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_DASHING - , - domain_id, - security_options -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - , - domain_id, - security_options, - localhost_only -#elif RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_FOXY - , - domain_id, - localhost_only -#endif /* RMW_CONNEXT_RELEASE */ - ); + context, name, ns, domain_id, security_options, localhost_only); } @@ -337,14 +310,11 @@ rmw_node_get_graph_guard_condition(const rmw_node_t * rmw_node) return rmw_api_connextdds_node_get_graph_guard_condition(rmw_node); } -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - rmw_ret_t rmw_node_assert_liveliness(const rmw_node_t * node) { return rmw_api_connextdds_node_assert_liveliness(node); } -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ /***************************************************************************** * Publication API @@ -385,21 +355,11 @@ rmw_publish_loaned_message( rmw_ret_t rmw_init_publisher_allocation( const rosidl_message_type_support_t * type_support, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT const rosidl_message_bounds_t * message_bounds, -#else - const rosidl_runtime_c__Sequence__bound * message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ rmw_publisher_allocation_t * allocation) { return rmw_api_connextdds_init_publisher_allocation( - type_support, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - message_bounds, -#else - message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ - allocation); + type_support, message_bounds, allocation); } @@ -416,19 +376,11 @@ rmw_create_publisher( const rmw_node_t * node, const rosidl_message_type_support_t * type_supports, const char * topic_name, - const rmw_qos_profile_t * qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , - const rmw_publisher_options_t * publisher_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -) + const rmw_qos_profile_t * qos_policies, + const rmw_publisher_options_t * publisher_options) { return rmw_api_connextdds_create_publisher( - node, type_supports, topic_name, qos_policies -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - , publisher_options -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - ); + node, type_supports, topic_name, qos_policies, publisher_options); } @@ -518,21 +470,11 @@ rmw_destroy_publisher( rmw_ret_t rmw_get_serialized_message_size( const rosidl_message_type_support_t * type_supports, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT const rosidl_message_bounds_t * message_bounds, -#else - const rosidl_runtime_c__Sequence__bound * message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ size_t * size) { return rmw_api_connextdds_get_serialized_message_size( - type_supports, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - message_bounds, -#else - message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ - size); + type_supports, message_bounds, size); } @@ -564,21 +506,13 @@ rmw_deserialize( rmw_ret_t rmw_take_response( const rmw_client_t * client, -#if RMW_CONNEXT_HAVE_SERVICE_INFO - rmw_service_info_t * request_header, -#else rmw_request_id_t * request_header, -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ void * ros_response, bool * taken) { return rmw_api_connextdds_take_response( client, -#if RMW_CONNEXT_HAVE_SERVICE_INFO - request_header, -#else request_header, -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ ros_response, taken); } @@ -587,21 +521,13 @@ rmw_take_response( rmw_ret_t rmw_take_request( const rmw_service_t * service, -#if RMW_CONNEXT_HAVE_SERVICE_INFO - rmw_service_info_t * request_header, -#else rmw_request_id_t * request_header, -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ void * ros_request, bool * taken) { return rmw_api_connextdds_take_request( service, -#if RMW_CONNEXT_HAVE_SERVICE_INFO request_header, -#else - request_header, -#endif /* RMW_CONNEXT_HAVE_SERVICE_INFO */ ros_request, taken); } @@ -674,21 +600,11 @@ rmw_destroy_service( rmw_ret_t rmw_init_subscription_allocation( const rosidl_message_type_support_t * type_support, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT const rosidl_message_bounds_t * message_bounds, -#else - const rosidl_runtime_c__Sequence__bound * message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ rmw_subscription_allocation_t * allocation) { return rmw_api_connextdds_init_subscription_allocation( - type_support, -#if RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT - message_bounds, -#else - message_bounds, -#endif /* RMW_CONNEXT_RELEASE <= RMW_CONNEXT_RELEASE_ELOQUENT */ - allocation); + type_support, message_bounds, allocation); } @@ -706,21 +622,10 @@ rmw_create_subscription( const rosidl_message_type_support_t * type_supports, const char * topic_name, const rmw_qos_profile_t * qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - const rmw_subscription_options_t * subscription_options -#else - bool ignore_local_publications -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ -) + const rmw_subscription_options_t * subscription_options) { return rmw_api_connextdds_create_subscription( - node, type_supports, topic_name, qos_policies, -#if RMW_CONNEXT_HAVE_OPTIONS_PUBSUB - subscription_options -#else - ignore_local_publications -#endif /* RMW_CONNEXT_HAVE_OPTIONS_PUBSUB */ - ); + node, type_supports, topic_name, qos_policies, subscription_options); } @@ -775,24 +680,6 @@ rmw_take_with_info( subscription, ros_message, taken, message_info, allocation); } -#if RMW_CONNEXT_HAVE_TAKE_SEQ - -rmw_ret_t -rmw_take_sequence( - const rmw_subscription_t * subscription, - size_t count, - rmw_message_sequence_t * message_sequence, - rmw_message_info_sequence_t * message_info_sequence, - size_t * taken, - rmw_subscription_allocation_t * allocation) -{ - return rmw_api_connextdds_take_sequence( - subscription, count, message_sequence, message_info_sequence, - taken, allocation); -} - -#endif /* RMW_CONNEXT_HAVE_TAKE_SEQ */ - rmw_ret_t rmw_take_serialized_message( @@ -910,24 +797,3 @@ rmw_wait( return rmw_api_connextdds_wait( subs, gcs, srvs, cls, evs, wait_set, wait_timeout); } - -/****************************************************************************** - * QoS Profile functions - ******************************************************************************/ -#if RMW_CONNEXT_HAVE_QOS_PROFILE_API -rmw_ret_t -rmw_qos_profile_check_compatible( - const rmw_qos_profile_t publisher_profile, - const rmw_qos_profile_t subscription_profile, - rmw_qos_compatibility_type_t * compatibility, - char * reason, - size_t reason_size) -{ - return rmw_api_connextdds_qos_profile_check_compatible( - publisher_profile, - subscription_profile, - compatibility, - reason, - reason_size); -} -#endif /* RMW_CONNEXT_HAVE_QOS_PROFILE_API */ diff --git a/rmw_dds_common/.github/ISSUE_TEMPLATE.md b/rmw_dds_common/.github/ISSUE_TEMPLATE.md new file mode 100644 index 00000000..f705392b --- /dev/null +++ b/rmw_dds_common/.github/ISSUE_TEMPLATE.md @@ -0,0 +1,45 @@ + + +## Bug report + +**Required Info:** + +- Operating System: + - +- Installation type: + - +- Version or commit hash: + - +- DDS implementation: + - +- Client library (if applicable): + - + +#### Steps to reproduce issue + +``` + +``` + +#### Expected behavior + +#### Actual behavior + +#### Additional information + + +---- +## Feature request + +#### Feature description + + +#### Implementation considerations + diff --git a/rmw_dds_common/CONTRIBUTING.md b/rmw_dds_common/CONTRIBUTING.md new file mode 100644 index 00000000..cfba094d --- /dev/null +++ b/rmw_dds_common/CONTRIBUTING.md @@ -0,0 +1,18 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). diff --git a/rmw_dds_common/LICENSE b/rmw_dds_common/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/rmw_dds_common/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/rmw_dds_common/README.md b/rmw_dds_common/README.md new file mode 100644 index 00000000..686d534c --- /dev/null +++ b/rmw_dds_common/README.md @@ -0,0 +1,17 @@ +# rmw_dds_common: ROS 2 C++ utilities for DDS-based RMWs + +`rmw_dds_common` is a collection of C++ APIs to support DDS based, C++ RMW implementations. + +## Quality Declaration + +This package claims to be in the **Quality Level 1** category, see the [Quality Declaration](rmw_dds_common/QUALITY_DECLARATION.md) for more details. + +## API + +This package contains: + +- Generic graph cache +- Common discovery messages +- Data types and utilities + +See [feature list](rmw_dds_common/docs/FEATURES.md) for further reference. diff --git a/rmw_dds_common/rmw_dds_common/CHANGELOG.rst b/rmw_dds_common/rmw_dds_common/CHANGELOG.rst new file mode 100644 index 00000000..7a65cf45 --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/CHANGELOG.rst @@ -0,0 +1,36 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rmw_dds_common +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.2 (2020-11-11) +------------------ +* Update QD links to Foxy +* Updated performance section QD (`#30 `_) +* Update Quality Declaration to QL2 (`#31 `_) +* Add fault injection macro unit tests (`#27 `_) +* Added benchmark test to rmw_dds_common (`#29 `_) +* Contributors: Alejandro Hernández Cordero, Michel Hidalgo, Stephen Brawner + +1.0.1 (2020-06-01) +------------------ +* Add Security Vulnerability Policy pointing to REP-2006 (`#21 `_) +* Fix graph cache tests (`#22 `_) +* Contributors: Chris Lalancette, Michel Hidalgo + +1.0.0 (2020-05-26) +------------------ +* Added Doxyfile (`#19 `_) +* Improve test coverage. (`#20 `_) +* Add README and QUALITY_DECLARATION for current QL level (`#17 `_) +* Contributors: Alejandro Hernández Cordero, Michel Hidalgo, Stephen Brawner + +0.1.0 (2020-04-25) +------------------ +* Export targets in addition to include directories / libraries (`#15 `_) +* Increasing code coverage (`#14 `_) +* security-context -> enclave (`#13 `_) +* Make rmw_dds_common use rosidl_generator_interfaces normally (`#12 `_) +* Changed rosidl_generator_cpp with rosidl_runtime_cpp (`#10 `_) +* Fix windows warning (`#7 `_) +* First implementation (`#4 `_) +* Contributors: Alejandro Hernández Cordero, Dirk Thomas, Ivan Santiago Paunovic, Mikael Arguedas diff --git a/rmw_dds_common/rmw_dds_common/CMakeLists.txt b/rmw_dds_common/rmw_dds_common/CMakeLists.txt new file mode 100644 index 00000000..2037bbf7 --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/CMakeLists.txt @@ -0,0 +1,99 @@ +cmake_minimum_required(VERSION 3.5) + +project(rmw_dds_common) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rcpputils REQUIRED) +find_package(rcutils REQUIRED) +find_package(rmw REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +ament_export_dependencies(ament_cmake_core) +ament_export_dependencies(rcpputils) +ament_export_dependencies(rcutils) +ament_export_dependencies(rmw) +ament_export_dependencies(rosidl_default_runtime) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/Gid.msg" + "msg/NodeEntitiesInfo.msg" + "msg/ParticipantEntitiesInfo.msg" +) + +add_library(${PROJECT_NAME}_library SHARED + src/gid_utils.cpp + src/graph_cache.cpp) +# set_target_properties(${PROJECT_NAME}_library +# PROPERTIES OUTPUT_NAME ${PROJECT_NAME}) +ament_target_dependencies(${PROJECT_NAME}_library + "rcpputils" + "rcutils" + "rmw" + "rosidl_runtime_cpp") +add_dependencies(${PROJECT_NAME}_library + ${PROJECT_NAME}) +target_include_directories(${PROJECT_NAME}_library + PUBLIC + "$" + "$" + "$" + "$") + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME}_library + PRIVATE "RMW_DDS_COMMON_BUILDING_LIBRARY") + +install( + TARGETS ${PROJECT_NAME}_library EXPORT ${PROJECT_NAME}_library + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}_library) +# ament_export_targets(${PROJECT_NAME}_library) + +install( + DIRECTORY include/ + DESTINATION include) + +# if(BUILD_TESTING) +# find_package(ament_lint_auto REQUIRED) +# find_package(ament_cmake_gmock REQUIRED) +# find_package(osrf_testing_tools_cpp REQUIRED) +# ament_lint_auto_find_test_dependencies() + +# find_package(performance_test_fixture REQUIRED) +# # Give cppcheck hints about macro definitions coming from outside this package +# get_target_property(ament_cmake_cppcheck_ADDITIONAL_INCLUDE_DIRS performance_test_fixture::performance_test_fixture INTERFACE_INCLUDE_DIRECTORIES) + +# ament_add_gmock(test_graph_cache test/test_graph_cache.cpp) +# if(TARGET test_graph_cache) +# target_link_libraries(test_graph_cache ${PROJECT_NAME}_library osrf_testing_tools_cpp::memory_tools) +# target_compile_definitions(test_graph_cache PUBLIC RCUTILS_ENABLE_FAULT_INJECTION) +# endif() + +# ament_add_gmock(test_gid_utils test/test_gid_utils.cpp) +# if(TARGET test_gid_utils) +# target_link_libraries(test_gid_utils ${PROJECT_NAME}_library) +# endif() + +# add_performance_test(benchmark_graph_cache test/benchmark/benchmark_graph_cache.cpp) +# if(TARGET benchmark_graph_cache) +# target_link_libraries(benchmark_graph_cache ${PROJECT_NAME}_library) +# endif() +# endif() + +ament_package() diff --git a/rmw_dds_common/rmw_dds_common/Doxyfile b/rmw_dds_common/rmw_dds_common/Doxyfile new file mode 100644 index 00000000..bcb093ad --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/Doxyfile @@ -0,0 +1,30 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "rmw_dds_common" +PROJECT_NUMBER = master +PROJECT_BRIEF = "Define a common interface between DDS implementations of ROS middleware." + +INPUT = ./include ../README.md +USE_MDFILE_AS_MAINPAGE = ../README.md +RECURSIVE = YES +OUTPUT_DIRECTORY = doc_output + +EXTRACT_ALL = YES +SORT_MEMBER_DOCS = NO + +GENERATE_LATEX = NO + +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +PREDEFINED += RMW_DDS_COMMON_PUBLIC= + +# Tag files that do not exist will produce a warning and cross-project linking will not work. +TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +# Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) +TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +TAGFILES += "../../../../doxygen_tag_files/rcpputils.tag=http://docs.ros2.org/latest/api/rcpputils/" +TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +TAGFILES += "../../../../doxygen_tag_files/rosidl_runtime_cpp.tag=http://docs.ros2.org/latest/api/rosidl_runtime_cpp/" +# Uncomment to generate tag files for cross-project linking. +GENERATE_TAGFILE = "../../../../doxygen_tag_files/rmw_dds_common.tag" diff --git a/rmw_dds_common/rmw_dds_common/QUALITY_DECLARATION.md b/rmw_dds_common/rmw_dds_common/QUALITY_DECLARATION.md new file mode 100644 index 00000000..fb8ade04 --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/QUALITY_DECLARATION.md @@ -0,0 +1,208 @@ +This document is a declaration of software quality for the `rmw_dds_common` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). + +# `rmw_dds_common` Quality Declaration + +The package `rmw_dds_common` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware. + +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 1 in REP-2004](https://www.ros.org/reps/rep-2004.html). + +## Version Policy [1] + +### Version Scheme [1.i] + +`rmw_dds_common` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#versioning). + +### Version Stability [1.ii] + +`rmw_dds_common` is at a stable version, i.e. `>= 1.0.0`. +The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). + +### Public API Declaration [1.iii] + +All symbols in the installed headers and message files (.msg) are considered part of the public API. + +All installed headers are in the `include` directory of the package, headers in any other folders are not installed and considered private. +All installed message files are in the `msg` directory of the package. + +### API Stability Within a Released ROS Distribution [1.iv]/[1.vi] + +`rmw_dds_common` will not break public API within a released ROS distribution, i.e. no major releases once the ROS distribution is released. + +### ABI Stability Within a Released ROS Distribution [1.v]/[1.vi] + +`rmw_dds_common` contains C and C++ code and therefore must be concerned with ABI stability, and will maintain ABI stability within a ROS distribution. + +## Change Control Process [2] + +`rmw_dds_common` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#package-requirements). + +### Change Requests [2.i] + +This package requires that all changes occur through a pull request. + +### Contributor Origin [2.ii] + +This package uses DCO as its confirmation of contributor origin policy. More information can be found in [CONTRIBUTING](../CONTRIBUTING.md). + +### Peer Review Policy [2.iii] + +Following the recommended guidelines for ROS Core packages, all pull requests must have at least 1 peer review. + +### Continuous Integration [2.iv] + +All pull request must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers) + +### Documentation Policy [2.v] + +All pull requests must resolve related documentation changes before merging. + +## Documentation [3] + +### Feature Documentation [3.i] + +`rmw_dds_common` has a documented [feature list](docs/FEATURES.md). + +### Public API Documentation [3.ii] + +`rmw_dds_common` has embedded API documentation. It is not yet hosted publicly. + +### License [3.iii] + +The license for `rmw_dds_common` is Apache 2.0, and a summary is in each source file, the type is declared in the `package.xml` manifest file, and a full copy of the license is in the [LICENSE](../LICENSE) file. + +There is an automated test which runs a linter that ensures each file has a license statement. + +Most recent test results can be found [here](http://build.ros2.org/view/Fpr/job/Fpr__rmw_dds_common__ubuntu_focal_amd64/lastCompletedBuild/testReport/) + +### Copyright Statements [3.iv] + +The copyright holders each provide a statement of copyright in each source code file in `rmw_dds_common`. + +There is an automated test which runs a linter that ensures each file has at least one copyright statement. + +The results of the test can be found [here](http://build.ros2.org/view/Fpr/job/Fpr__rmw_dds_common__ubuntu_focal_amd64/lastCompletedBuild/testReport/). + +## Testing [4] + +### Feature Testing [4.i] + +Each feature in `rmw_dds_common` has tests which simulate typical usage, and they are located in the [test](./test) directory. +New features are required to have tests before being added. + +### Public API Testing [4.ii] + +Each part of the public API has tests, which are located in the [test](./test) directory. +New additions or changes to the public API require tests before being added. +The tests aim to cover both typical usage and corner cases, but are quantified by contributing to code coverage. + +### Coverage [4.iii] + +`rmw_dds_common` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#coverage), and opts to use branch coverage instead of line coverage. + +This includes: + +- tracking and reporting line coverage statistics +- achieving and maintaining a reasonable branch line coverage (90-100%) +- no lines are manually skipped in coverage calculations + +Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. + +Current coverage statistics can be viewed [here](https://ci.ros2.org/job/nightly_linux_foxy_coverage/lastCompletedBuild/cobertura/src_ros2_rmw_dds_common_rmw_dds_common_include_rmw_dds_common/) and [here](https://ci.ros2.org/job/nightly_linux_foxy_coverage/lastCompletedBuild/cobertura/src_ros2_rmw_dds_common_rmw_dds_common_src/). + +A summary of how these statistics are calculated can be found in the [ROS 2 On-boarding guide](https://index.ros.org/doc/ros2/Contributing/ROS-2-On-boarding-Guide/#note-on-coverage-runs). + +### Performance [4.iv] + +`rmw_dds_common` follows the recommendations for performance testing of C code in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#performance), and opts to do performance analysis on each release rather than each change. + +The performance tests of `rmw_dds_common` are located in the [test/benchmark directory](https://github.com/ros2/rmw_dds_common/tree/foxy/rmw_dds_common/test/benchmark). + +Package and system level performance benchmarks that cover features of `rmw_dds_common` can be found at: +* [Benchmarks](http://build.ros2.org/view/Fci/job/Fci__benchmark_ubuntu_focal_amd64/BenchmarkTable/) +* [Performance](http://build.ros2.org/view/Fci/job/Fci__nightly-performance_ubuntu_focal_amd64/lastCompletedBuild/) + +Changes that introduce regressions in performance must be adequately justified in order to be accepted and merged. + +### Linters and Static Analysis [4.v] + +`rmw_dds_common` uses and passes all the standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/#linters). + +Results of the nightly linter tests can be found [here](http://build.ros2.org/view/Fpr/job/Fpr__rmw_dds_common__ubuntu_focal_amd64/lastCompletedBuild/testReport/). + +## Dependencies [5] + +### Direct Runtime ROS Dependencies [5.i]/[5.ii] + +`rmw_dds_common` has the following runtime ROS dependencies which are at **Quality Level 1** +* `rcutils`: [QUALITY DECLARATION](https://github.com/ros2/rcutils/blob/foxy/QUALITY_DECLARATION.md) +* `rcpputils`: [QUALITY DECLARATION](https://github.com/ros2/rcpputils/blob/foxy/QUALITY_DECLARATION.md) +* `rmw`: [QUALITY DECLARATION](https://github.com/ros2/rmw/blob/foxy/rmw/QUALITY_DECLARATION.md) +* `rosidl_default_runtime`: [QUALITY DECLARATION](https://github.com/ros2/rosidl_defaults/blob/foxy/rosidl_default_runtime/QUALITY_DECLARATION.md) +* `rosidl_runtime_cpp`: [QUALITY DECLARATION](https://github.com/ros2/rosidl/blob/foxy/rosidl_runtime_cpp/QUALITY_DECLARATION.md) + +It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. +It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. + +### Direct Runtime Non-ROS Dependencies [5.iii] + +`rmw_dds_common` does not have any runtime non-ROS dependencies. + +## Platform Support [6] + +`rmw_dds_common` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them. + +Though there are no nightly jobs for foxy outside of linux, each change is tested on ci.ros2.org. +* [linux-aarch64](https://ci.ros2.org/job/ci_linux-aarch64) +* [linux](https://ci.ros2.org/job/ci_linux) +* [mac_osx](https://ci.ros2.org/job/ci_osx) +* [windows](https://ci.ros2.org/job/ci_windows) + +## Security [7] + +### Vulnerability Disclosure Policy [7.i] + +This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). + +# Current status Summary + +The chart below compares the requirements in the REP-2004 with the current state of the `rmw_dds_common` package. + +|Number| Requirement| Current state | +|--|--|--| +|1| **Version policy** |---| +|1.i|Version Policy available | ✓ | +|1.ii|Stable version |✓ | +|1.iii|Declared public API|✓| +|1.iv|API stability policy|✓| +|1.v|ABI stability policy|✓| +|1.vi_|API/ABI stable within ros distribution|✓| +|2| **Change control process** |---| +|2.i| All changes occur on change request | ✓| +|2.ii| Contributor origin (DCO, CLA, etc) | ✓| +|2.iii| Peer review policy | ✓ | +|2.iv| CI policy for change requests | ✓ | +|2.v| Documentation policy for change requests | ✓ | +|3| **Documentation** | --- | +|3.i| Per feature documentation | ✓ | +|3.ii| Per public API item documentation | * | +|3.iii| Declared License(s) | ✓ | +|3.iv| Copyright in source files| ✓ | +|3.v.a| Quality declaration linked to README | ✓ | +|3.v.b| Centralized declaration available for peer review |✓| +|4| **Testing** | --- | +|4.i| Feature items tests | ✓ | +|4.ii| Public API tests | ✓ | +|4.iii.a| Using coverage | ✓ | +|4.iii.a| Coverage policy | ✓ | +|4.iv.a| Performance tests (if applicable) | ✓ | +|4.iv.b| Performance tests policy| ✓ | +|4.v.a| Code style enforcement (linters)| ✓ | +|4.v.b| Use of static analysis tools | ✓ | +|5| **Dependencies** | --- | +|5.i| Must not have ROS lower level dependencies | ✓ | +|5.ii| Optional ROS lower level dependencies| ✓ | +|5.iii| Justifies quality use of non-ROS dependencies |✓| +|6| **Platform support** | --- | +|6.i| Support targets Tier1 ROS platforms| ✓ | +|7| **Security** | --- | +|7.i| Vulnerability Disclosure Policy | ✓ | diff --git a/rmw_dds_common/rmw_dds_common/docs/FEATURES.md b/rmw_dds_common/rmw_dds_common/docs/FEATURES.md new file mode 100644 index 00000000..a29114a2 --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/docs/FEATURES.md @@ -0,0 +1,13 @@ +# rmw_dds_common Features + +This package includes: + +- A generic [`GraphCache`](rmw_dds_common/include/rmw_dds_common/graph_cache.hpp) to track DDS entities such as participants and data readers and writers, plus ROS nodes as an additional abstraction for any DDS based `rmw` implementation to use +- Common messages to communicate [ROS nodes discovery information](https://github.com/ros2/design/pull/250): + - [`rmw_dds_common/msg/Gid`](rmw_dds_common/msg/Gid.msg) + - [`rmw_dds_common/msg/NodeEntitiesInfo`](rmw_dds_common/msg/NodeEntitiesInfo.msg) + - [`rmw_dds_common/msg/ParticipantEntitiesInfo`](rmw_dds_common/msg/ParticipantEntitiesInfo.msg) +- Some useful data types and utilities: + - A generic [`Context`](rmw_dds_common/include/rmw_dds_common/context.hpp) type to withhold most state needed to implement [ROS nodes discovery](https://github.com/ros2/design/pull/250) + - [Comparison utilities and some C++ operator overloads](rmw_dds_common/include/rmw_dds_common/gid_utils.hpp) for `rmw_gid_t` instances + - [Conversion utilities](rmw_dds_common/include/rmw_dds_common/gid_utils.hpp) between `rmw_dds_common/msg/Gid` messages and `rmw_gid_t` instances diff --git a/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/context.hpp b/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/context.hpp new file mode 100644 index 00000000..2a076ab6 --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/context.hpp @@ -0,0 +1,45 @@ +// 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 RMW_DDS_COMMON__CONTEXT_HPP_ +#define RMW_DDS_COMMON__CONTEXT_HPP_ + +#include +#include +#include + +#include "rmw/types.h" + +#include "rmw_dds_common/graph_cache.hpp" +#include "rmw_dds_common/visibility_control.h" + +namespace rmw_dds_common +{ + +struct Context +{ + rmw_gid_t gid; + rmw_publisher_t * pub; + rmw_subscription_t * sub; + GraphCache graph_cache; + std::mutex node_update_mutex; + std::thread listener_thread; + std::atomic_bool thread_is_running; + rmw_guard_condition_t * listener_thread_gc; + rmw_guard_condition_t * graph_guard_condition; +}; + +} // namespace rmw_dds_common + +#endif // RMW_DDS_COMMON__CONTEXT_HPP_ diff --git a/rmw_connextdds_common/include/rmw_connextdds/gid_utils.hpp b/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/gid_utils.hpp similarity index 63% rename from rmw_connextdds_common/include/rmw_connextdds/gid_utils.hpp rename to rmw_dds_common/rmw_dds_common/include/rmw_dds_common/gid_utils.hpp index 99defa63..9b29bb03 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/gid_utils.hpp +++ b/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/gid_utils.hpp @@ -12,44 +12,30 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_CONNEXTDDS__GID_UTILS_HPP_ -#define RMW_CONNEXTDDS__GID_UTILS_HPP_ - -#include "rmw_connextdds/static_config.hpp" - -#if !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON +#ifndef RMW_DDS_COMMON__GID_UTILS_HPP_ +#define RMW_DDS_COMMON__GID_UTILS_HPP_ #include "rmw/types.h" -#include "rmw_connextdds/visibility_control.h" -#include "rmw_connextdds_common/msg/gid.hpp" -#include "rmw_connextdds_common/msg/node_entities_info.hpp" -#include "rmw_connextdds_common/msg/participant_entities_info.hpp" +#include "rmw_dds_common/visibility_control.h" +#include "rmw_dds_common/msg/gid.hpp" namespace rmw_dds_common { -namespace msg -{ -using Gid = rmw_connextdds_common::msg::Gid; -using NodeEntitiesInfo = rmw_connextdds_common::msg::NodeEntitiesInfo; -using ParticipantEntitiesInfo = rmw_connextdds_common::msg::ParticipantEntitiesInfo; -} - /// Comparator for rmw_gid_t, in order to use them as a key of a map -struct RMW_CONNEXTDDS_PUBLIC_TYPE Compare_rmw_gid_t +struct RMW_DDS_COMMON_PUBLIC_TYPE Compare_rmw_gid_t { - /// Compare lhs with rhs. bool operator()(const rmw_gid_t & lhs, const rmw_gid_t & rhs) const; }; /// Stream operator for rmw_gid_t -RMW_CONNEXTDDS_PUBLIC +RMW_DDS_COMMON_PUBLIC std::ostream & operator<<(std::ostream & ostream, const rmw_gid_t & gid); /// operator== for rmw_gid_t -RMW_CONNEXTDDS_PUBLIC +RMW_DDS_COMMON_PUBLIC bool operator==(const rmw_gid_t & lhs, const rmw_gid_t & rhs); @@ -57,7 +43,7 @@ operator==(const rmw_gid_t & lhs, const rmw_gid_t & rhs); /** * For internal usage, both pointers are assumed to be valid. */ -RMW_CONNEXTDDS_PUBLIC +RMW_DDS_COMMON_PUBLIC void convert_gid_to_msg( const rmw_gid_t * gid, @@ -67,7 +53,7 @@ convert_gid_to_msg( /** * For internal usage, both pointers are supposed to be valid. */ -RMW_CONNEXTDDS_PUBLIC +RMW_DDS_COMMON_PUBLIC void convert_msg_to_gid( const rmw_dds_common::msg::Gid * msg_gid, @@ -75,6 +61,4 @@ convert_msg_to_gid( } // namespace rmw_dds_common -#endif /* !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON */ - -#endif // RMW_CONNEXTDDS__GID_UTILS_HPP_ +#endif // RMW_DDS_COMMON__GID_UTILS_HPP_ diff --git a/rmw_connextdds_common/include/rmw_connextdds/graph_cache_common.hpp b/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/graph_cache.hpp similarity index 85% rename from rmw_connextdds_common/include/rmw_connextdds/graph_cache_common.hpp rename to rmw_dds_common/rmw_dds_common/include/rmw_dds_common/graph_cache.hpp index e8d31d64..7a9aaedb 100644 --- a/rmw_connextdds_common/include/rmw_connextdds/graph_cache_common.hpp +++ b/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/graph_cache.hpp @@ -12,12 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_CONNEXTDDS__GRAPH_CACHE_COMMON_HPP_ -#define RMW_CONNEXTDDS__GRAPH_CACHE_COMMON_HPP_ - -#include "rmw_connextdds/static_config.hpp" - -#if !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON +#ifndef RMW_DDS_COMMON__GRAPH_CACHE_HPP_ +#define RMW_DDS_COMMON__GRAPH_CACHE_HPP_ #include #include @@ -30,19 +26,15 @@ #include "rcutils/logging_macros.h" #include "rmw/names_and_types.h" -#include "rmw_connextdds/topic_endpoint_info.h" -#include "rmw_connextdds/topic_endpoint_info_array.h" +#include "rmw_dds_common/topic_endpoint_info.h" +#include "rmw_dds_common/topic_endpoint_info_array.h" #include "rmw/types.h" -#include "rmw_connextdds/gid_utils.hpp" -#include "rmw_connextdds/visibility_control.h" -#include "rmw_connextdds_common/msg/gid.hpp" -#include "rmw_connextdds_common/msg/node_entities_info.hpp" -#include "rmw_connextdds_common/msg/participant_entities_info.hpp" - -/// Failed to find node name -// Using same return code than in rcl -#define RMW_RET_NODE_NAME_NON_EXISTENT 203 +#include "rmw_dds_common/gid_utils.hpp" +#include "rmw_dds_common/visibility_control.h" +#include "rmw_dds_common/msg/gid.hpp" +#include "rmw_dds_common/msg/node_entities_info.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" namespace rmw_dds_common { @@ -58,7 +50,7 @@ struct ParticipantInfo; class GraphCache { friend - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC std::ostream & operator<<(std::ostream & ostream, const GraphCache & topic_cache); @@ -76,26 +68,26 @@ class GraphCache } /// Clear previously registered "on change" callback. - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC void clear_on_change_callback(); /** - * \defgroup dds_discovery_api dds_discovery_api + * \defgroup dds_discovery_api * Methods used to update the Graph Cache based on DDS discovery. * @{ */ /// Add a data writer based on discovery. /** - * \param writer_gid The data writer guid. + * \param gid The data writer guid. * \param topic_name * \param type_name * \param participant_gid gid of the participant. * \param qos Quality of service of the writer. * \return `true` when a change took place. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC bool add_writer( const rmw_gid_t & writer_gid, @@ -106,14 +98,14 @@ class GraphCache /// Add a data reader based on discovery. /** - * \param reader_gid The data reader guid + * \param gid The data reader guid * \param topic_name * \param type_name * \param participant_gid gid of the participant. * \param qos Quality of service of the writer. * \return `true` when a change took place. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC bool add_reader( const rmw_gid_t & reader_gid, @@ -132,7 +124,7 @@ class GraphCache * \param is_reader Adds a reader when `true`, if not a writer. * \return `true` when a change took place. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC bool add_entity( const rmw_gid_t & gid, @@ -147,7 +139,7 @@ class GraphCache * \param gid The data writer guid. * \return `true` when a change took place. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC bool remove_writer(const rmw_gid_t & gid); @@ -156,7 +148,7 @@ class GraphCache * \param gid The data reader guid. * \return `true` when a change took place. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC bool remove_reader(const rmw_gid_t & gid); @@ -166,23 +158,23 @@ class GraphCache * \param is_reader `true` for removing a data reader. * \return `true` when a change took place. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC bool remove_entity(const rmw_gid_t & gid, bool is_reader); /** * @} - * \defgroup common_api common_api + * \defgroup common_api * Methods used to update the Graph Cache. * @{ */ /// Add a discovered participant to the cache. /** - * \param participant_gid The participant guid. + * \param gid The participant guid. * \param enclave Name of the enclave. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC void add_participant( const rmw_gid_t & participant_gid, @@ -193,13 +185,13 @@ class GraphCache * \param participant_gid * \return `true` when a change took place. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC bool remove_participant(const rmw_gid_t & participant_gid); /** * @} - * \defgroup ros_discovery_api ros_discovery_api + * \defgroup ros_discovery_api * Methods used to update the Graph Cache based on ROS 2 specific messages received from * remote participants. * @{ @@ -209,25 +201,25 @@ class GraphCache /** * \param msg Will be filled with the received `ParticipantEntitiesInfo` message. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC void update_participant_entities(const rmw_dds_common::msg::ParticipantEntitiesInfo & msg); /** * @} - * \defgroup local_api local_api + * \defgroup local_api * Methods used to update the Graph Cache, based on local construction and destruction of objects. * @{ */ /// Add a node to the graph, and get the message to be sent. /** - * \param participant_gid participant GUID. + * \param gid participant GUID. * \param node_name name of the node to be added. * \param node_namespace node namespace. * \return message to be sent. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_dds_common::msg::ParticipantEntitiesInfo add_node( const rmw_gid_t & participant_gid, @@ -236,12 +228,12 @@ class GraphCache /// Remove a node to the graph, and get the message to be sent. /** - * \param participant_gid participant GUID. + * \param gid participant GUID. * \param node_name name of the node to be added. * \param node_namespace node namespace. * \return message to be sent. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_dds_common::msg::ParticipantEntitiesInfo remove_node( const rmw_gid_t & participant_gid, @@ -256,7 +248,7 @@ class GraphCache * \param node_namespace Node namespace. * \return Message to be sent. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_dds_common::msg::ParticipantEntitiesInfo associate_writer( const rmw_gid_t & writer_gid, @@ -272,7 +264,7 @@ class GraphCache * \param node_namespace Node namespace. * \return Message to be sent. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_dds_common::msg::ParticipantEntitiesInfo dissociate_writer( const rmw_gid_t & writer_gid, @@ -288,7 +280,7 @@ class GraphCache * \param node_namespace Node namespace. * \return Message to be sent. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_dds_common::msg::ParticipantEntitiesInfo associate_reader( const rmw_gid_t & reader_gid, @@ -304,7 +296,7 @@ class GraphCache * \param node_namespace Node namespace. * \return Message to be sent. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_dds_common::msg::ParticipantEntitiesInfo dissociate_reader( const rmw_gid_t & reader_gid, @@ -314,7 +306,7 @@ class GraphCache /** * @} - * \defgroup introspection_api introspection_api + * \defgroup introspection_api * Methods used to introspect the GraphCache. * @{ */ @@ -328,7 +320,7 @@ class GraphCache * \return RMW_RET_ERROR if an unexpected error take place, or * \return RMW_RET_OK. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_ret_t get_writer_count( const std::string & topic_name, @@ -343,13 +335,12 @@ class GraphCache * \return RMW_RET_ERROR if an unexpected error take place, or * \return RMW_RET_OK. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_ret_t get_reader_count( const std::string & topic_name, size_t * count) const; - /// Callable used to demangle a name. using DemangleFunctionT = std::function; /// Get an array with information about the writers in a topic. @@ -363,7 +354,7 @@ class GraphCache * \return RMW_RET_ERROR if an unexpected error take place, or * \return RMW_RET_OK. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_ret_t get_writers_info_by_topic( const std::string & topic_name, @@ -382,7 +373,7 @@ class GraphCache * \return RMW_RET_ERROR if an unexpected error take place, or * \return RMW_RET_OK. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_ret_t get_readers_info_by_topic( const std::string & topic_name, @@ -396,7 +387,7 @@ class GraphCache * into a ros topic name. * \param[in] demangle_type Function that indicates how a dds type name is demangled * into a ros type name. - * \param[in] allocator + * \param[in] allocator. * \param[inout] topic_names_and_types A zero initialized names and types object, that * will be populated with the result. * @@ -406,7 +397,7 @@ class GraphCache * \return RMW_RET_ERROR if an unexpected error happened, or * \return RMW_RET_OK. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_ret_t get_names_and_types( DemangleFunctionT demangle_topic, @@ -417,7 +408,7 @@ class GraphCache /// Get the topic names and types that a node is publishing. /** * \param[in] node_name Name of the node. - * \param[in] namespace_ Namespace of the node. + * \param[in] node_namespace Namespace of the node. * \param[in] demangle_topic Function that indicates how a dds topic name is demangled * into a ros topic name. * \param[in] demangle_type Function that indicates how a dds type name is demangled @@ -432,7 +423,7 @@ class GraphCache * \return RMW_RET_ERROR if an unexpected error happened, or * \return RMW_RET_OK. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_ret_t get_writer_names_and_types_by_node( const std::string & node_name, @@ -445,12 +436,12 @@ class GraphCache /// Get the topic names and types that a node is subscribing. /** * \param[in] node_name Name of the node. - * \param[in] namespace_ Namespace of the node. + * \param[in] node_namespace Namespace of the node. * \param[in] demangle_topic Function that indicates how a dds topic name is demangled * into a ros topic name. * \param[in] demangle_type Function that indicates how a dds type name is demangled * into a ros type name. - * \param[in] allocator + * \param[in] allocator. * \param[inout] topic_names_and_types A zero initialized names and types object, that * will be populated with the result. * @@ -460,7 +451,7 @@ class GraphCache * \return RMW_RET_ERROR if an unexpected error happened, or * \return RMW_RET_OK. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_ret_t get_reader_names_and_types_by_node( const std::string & node_name, @@ -475,7 +466,7 @@ class GraphCache * \return RMW_RET_OK, or * \return RMW_RET_ERROR. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC size_t get_number_of_nodes() const; @@ -489,13 +480,13 @@ class GraphCache * \param[inout] enclaves A zero initialized string array, where the enclave * name of the node will be copied. Each item in this array corresponds to an item in the same * position of node_names array. In case is `nullptr`, it won't be used. - * \param[in] allocator + * \param[in] allocator. * \return RMW_RET_OK, or * \return RMW_RET_INVALID_ARGUMENT, or * \return RMW_RET_BAD_ALLOC, or * \return RMW_RET_ERROR. */ - RMW_CONNEXTDDS_PUBLIC + RMW_DDS_COMMON_PUBLIC rmw_ret_t get_node_names( rcutils_string_array_t * node_names, @@ -507,18 +498,10 @@ class GraphCache * @} */ - /// \internal - /// Sequence of NodeEntitiesInfo messages. using NodeEntitiesInfoSeq = decltype(std::declval().node_entities_info_seq); - /// \internal - /// Map from endpoint gids to endpoints discovery info. using EntityGidToInfo = std::map; - /// \internal - /// Map from participant gids to participant discovery info. using ParticipantToNodesMap = std::map; - /// \internal - /// Sequence of endpoints gids. using GidSeq = decltype(std::declval().writer_gid_seq); @@ -531,32 +514,23 @@ class GraphCache mutable std::mutex mutex_; }; -RMW_CONNEXTDDS_PUBLIC +RMW_DDS_COMMON_PUBLIC std::ostream & operator<<(std::ostream & ostream, const GraphCache & topic_cache); -/// Structure to represent the discovery data of a Participant. struct ParticipantInfo { - /// Discovery information of each Node created from a Participant. GraphCache::NodeEntitiesInfoSeq node_entities_info_seq; - /// Name of the enclave. std::string enclave; }; -/// Structure to represent the discovery data of an endpoint (reader or writer). struct EntityInfo { - /// Topic name. std::string topic_name; - /// Topic type. std::string topic_type; - /// Participant gid. rmw_gid_t participant_gid; - /// Quality of service of the topic. rmw_qos_profile_t qos; - /// Simple constructor. EntityInfo( const std::string & topic_name, const std::string & topic_type, @@ -571,6 +545,4 @@ struct EntityInfo } // namespace rmw_dds_common -#endif /* !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON */ - -#endif // RMW_CONNEXTDDS__GRAPH_CACHE_COMMON_HPP_ +#endif // RMW_DDS_COMMON__GRAPH_CACHE_HPP_ diff --git a/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/topic_endpoint_info.h b/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/topic_endpoint_info.h new file mode 100644 index 00000000..2d16b6f6 --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/topic_endpoint_info.h @@ -0,0 +1,345 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// 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 RMW_DDS_COMMON__TOPIC_ENDPOINT_INFO_H_ +#define RMW_DDS_COMMON__TOPIC_ENDPOINT_INFO_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcutils/allocator.h" +#include "rmw/types.h" +#include "rmw/visibility_control.h" + +/// Endpoint enumeration type +typedef enum RMW_PUBLIC_TYPE rmw_endpoint_type_t +{ + /// Endpoint type has not yet been set + RMW_ENDPOINT_INVALID = 0, + + /// Creates and publishes messages to the ROS topic + RMW_ENDPOINT_PUBLISHER, + + /// Listens for and receives messages from a topic + RMW_ENDPOINT_SUBSCRIPTION +} rmw_endpoint_type_t; + +/// A data structure that encapsulates the node name, node namespace, +/// topic_type, gid, and qos_profile of publishers and subscriptions +/// for a topic. +typedef struct RMW_PUBLIC_TYPE rmw_topic_endpoint_info_t +{ + /// Name of the node + const char * node_name; + /// Namespace of the node + const char * node_namespace; + /// The associated topic type + const char * topic_type; + /// The endpoint type + rmw_endpoint_type_t endpoint_type; + /// The GID of the endpoint + uint8_t endpoint_gid[RMW_GID_STORAGE_SIZE]; + /// QoS profile of the endpoint + rmw_qos_profile_t qos_profile; +} rmw_topic_endpoint_info_t; + +/// Return zero initialized topic endpoint info data structure. +/** + * Endpoint type will be invalid. + * Endpoint QoS profile will be the system default. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_topic_endpoint_info_t +rmw_get_zero_initialized_topic_endpoint_info(void); + +/// Finalize a topic endpoint info data structure. +/** + * This function deallocates all allocated members of the given data structure, + * and then zero initializes it. + * If a logical error, such as `RMW_RET_INVALID_ARGUMENT`, ensues, this function + * will return early, leaving the given data structure unchanged. + * Otherwise, it will proceed despite errors. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \par Thread-safety + * Finalization is a reentrant procedure, but: + * - Access to the topic endpoint info data structure is not synchronized. + * It is not safe to read or write `topic_endpoint` during finalization. + * - The default allocators are thread-safe objects, but any custom `allocator` may not be. + * Check your allocator documentation for further reference. + * + * \param[inout] topic_endpoint_info Data structure to be finalized. + * \param[in] allocator Allocator used to populate the given `topic_endpoint_info`. + * \returns `RMW_RET_OK` if successful, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `topic_endpoint_info` is NULL, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `allocator` is invalid, + * by rcutils_allocator_is_valid() definition, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + * \remark This function sets the RMW error state on failure. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_fini( + rmw_topic_endpoint_info_t * topic_endpoint_info, + rcutils_allocator_t * allocator); + +/// Set the topic type in the given topic endpoint info data structure. +/** + * This functions allocates memory and copies the value of the `topic_type` + * argument to set the data structure `topic_type` member. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \par Thread-safety + * Setting a member is a reentrant procedure, but: + * - Access to the topic endpoint info data structure is not synchronized. + * It is not safe to read or write the `topic_type` member of the given `topic_endpoint` + * while setting it. + * - Access to C-style string arguments is read-only but it is not synchronized. + * Concurrent `topic_type` reads are safe, but concurrent reads and writes are not. + * - The default allocators are thread-safe objects, but any custom `allocator` may not be. + * Check your allocator documentation for further reference. + * + * \pre Given `topic_type` is a valid C-style string i.e. NULL terminated. + * + * \param[inout] topic_endpoint_info Data structure to be populated. + * \param[in] topic_type Type name to be set. + * \param[in] allocator Allocator to be used. + * \returns `RMW_RET_OK` if successful, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `topic_endpoint_info` is NULL, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `topic_type` is NULL, or + * \returns `RMW_RET_BAD_ALLOC` if memory allocation fails, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + * \remark This function sets the RMW error state on failure. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_set_topic_type( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const char * topic_type, + rcutils_allocator_t * allocator); + +/// Set the node name in the given topic endpoint info data structure. +/** + * This functions allocates memory and copies the value of the `node_name` + * argument to set the data structure `node_name` member. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \par Thread-safety + * Setting a member is a reentrant procedure, but: + * - Access to the topic endpoint info data structure is not synchronized. + * It is not safe to read or write the `node_name` member of the given `topic_endpoint` + * while setting it. + * - Access to C-style string arguments is read-only but it is not synchronized. + * Concurrent `node_name` reads are safe, but concurrent reads and writes are not. + * - The default allocators are thread-safe objects, but any custom `allocator` may not be. + * Check your allocator documentation for further reference. + * + * \pre Given `node_name` is a valid C-style string i.e. NULL terminated. + * + * \param[inout] topic_endpoint_info Data structure to be populated. + * \param[in] node_name Node name to be set. + * \param[in] allocator Allocator to be used. + * \returns `RMW_RET_OK` if successful, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `topic_endpoint_info` is NULL, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `node_name` is NULL, or + * \returns `RMW_RET_BAD_ALLOC` if memory allocation fails, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + * \remark This function sets the RMW error state on failure. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_set_node_name( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const char * node_name, + rcutils_allocator_t * allocator); + +/// Set the node namespace in the given topic endpoint info data structure. +/** + * This functions allocates memory and copies the value of the `node_namespace` + * argument to set the data structure `node_namespace` member. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \par Thread-safety + * Setting a member is a reentrant procedure, but: + * - Access to the topic endpoint info data structure is not synchronized. + * It is not safe to read or write the `node_namespace` member of the given `topic_endpoint` + * while setting it. + * - Access to C-style string arguments is read-only but it is not synchronized. + * Concurrent `node_namespace` reads are safe, but concurrent reads and writes are not. + * - The default allocators are thread-safe objects, but any custom `allocator` may not be. + * Check your allocator documentation for further reference. + * + * \pre Given `node_namespace` is a valid C-style string i.e. NULL terminated. + * + * \param[inout] topic_endpoint_info Data structure to be populated. + * \param[in] node_namespace Node namespace to be set. + * \param[in] allocator Allocator to be used. + * \returns `RMW_RET_OK` if successful, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `topic_endpoint_info` is NULL, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `node_namespace` is NULL, or + * \returns `RMW_RET_BAD_ALLOC` if memory allocation fails, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + * \remark This function sets the RMW error state on failure. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_set_node_namespace( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const char * node_namespace, + rcutils_allocator_t * allocator); + +/// Set the endpoint type in the given topic endpoint info data structure. +/** + * This functions assigns the value of the `type` argument to the data structure + * `endpoint_type` member. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \par Thread-safety + * Setting a member is a reentrant procedure, but access to the + * topic endpoint info data structure is not synchronized. + * It is not safe to read or write the `endpoint_type` member of the + * given `topic_endpoint` while setting it. + * + * \param[inout] topic_endpoint_info Data structure to be populated. + * \param[in] type Endpoint type to be set. + * \returns `RMW_RET_OK` if successful, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `topic_endpoint_info` is NULL, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + * \remark This function sets the RMW error state on failure. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_set_endpoint_type( + rmw_topic_endpoint_info_t * topic_endpoint_info, + rmw_endpoint_type_t type); + +/// Set the endpoint gid in the given topic endpoint info data structure. +/** + * This functions copies the value of the `gid` argument to the data structure + * `endpoint_gid` member. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \par Thread-safety + * Setting a member is a reentrant procedure, but access to the + * topic endpoint info data structure is not synchronized. + * It is not safe to read or write the `gid` member of the + * given `topic_endpoint` while setting it. + * + * \param[inout] topic_endpoint_info Data structure to be populated. + * \param[in] gid Endpoint gid to be set. + * \param[in] size Size of the given `gid`. + * \returns `RMW_RET_OK` if successful, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `topic_endpoint_info` is NULL, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `gid` is NULL, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `size` is greater than RMW_GID_STORAGE_SIZE, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + * \remark This function sets the RMW error state on failure. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_set_gid( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const uint8_t gid[], + size_t size); + +/// Set the endpoint QoS profile in the given topic endpoint info data structure. +/** + * This functions assigns the value of the `qos_profile` argument to the data structure + * `qos_profile` member. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \par Thread-safety + * Setting a member is a reentrant procedure, but access to the + * topic endpoint info data structure is not synchronized. + * It is not safe to read or write the `qos_profile` member of the + * given `topic_endpoint` while setting it. + * + * \param[inout] topic_endpoint_info Data structure to be populated. + * \param[in] qos_profile QoS profile to be set. + * \returns `RMW_RET_OK` if successful, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `topic_endpoint_info` is NULL, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `qos_profile` is NULL, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + * \remark This function sets the RMW error state on failure. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_set_qos_profile( + rmw_topic_endpoint_info_t * topic_endpoint_info, + const rmw_qos_profile_t * qos_profile); + +#ifdef __cplusplus +} +#endif + +#endif // RMW_DDS_COMMON__TOPIC_ENDPOINT_INFO_H_ diff --git a/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/topic_endpoint_info_array.h b/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/topic_endpoint_info_array.h new file mode 100644 index 00000000..4687b6e8 --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/topic_endpoint_info_array.h @@ -0,0 +1,155 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// 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 RMW_DDS_COMMON__TOPIC_ENDPOINT_INFO_ARRAY_H_ +#define RMW_DDS_COMMON__TOPIC_ENDPOINT_INFO_ARRAY_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcutils/allocator.h" +#include "rmw_dds_common/topic_endpoint_info.h" +#include "rmw/visibility_control.h" + +/// Array of topic endpoint information +typedef struct RMW_PUBLIC_TYPE rmw_topic_endpoint_info_array_t +{ + /// Size of the array. + size_t size; + /// Contiguous storage for topic endpoint information elements. + rmw_topic_endpoint_info_t * info_array; +} rmw_topic_endpoint_info_array_t; + +/// Return a zero initialized array of topic endpoint information. +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_topic_endpoint_info_array_t +rmw_get_zero_initialized_topic_endpoint_info_array(void); + +/// Check that the given `topic_endpoint_info_array` is zero initialized. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \par Thread-safety + * Access to the array of topic endpoint information is read-only, but it is not synchronized. + * Concurrent `topic_endpoint_info_array` reads are safe, but concurrent reads + * and writes are not. + * + * \param[in] topic_endpoint_info_array Array to be checked. + * \returns `RMW_RET_OK` if array is zero initialized, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `topic_endpoint_info_array` is NULL, or + * \returns `RMW_RET_ERROR` if `topic_endpoint_info_array` is not zero initialized. + * \remark This function sets the RMW error state on failure. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_array_check_zero( + rmw_topic_endpoint_info_array_t * topic_endpoint_info_array); + +/// Initialize an array of topic endpoint information. +/** + * This function allocates space to hold `size` topic endpoint information elements. + * Both `info_array` and `size` members are updated accordingly. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \par Thread-safety + * Initialization is a reentrant procedure, but: + * - Access to the array of topic endpoint information is not synchronized. + * It is not safe to read or write `topic_endpoint_info_array` during initialization. + * - The default allocators are thread-safe objects, but any custom `allocator` may not be. + * Check your allocator documentation for further reference. + * + * \param[inout] topic_endpoint_info_array Array to be initialized on success, + * but left unchanged on failure. + * \param[in] size Size of the array. + * \param[in] allocator Allocator to be used to populate `names_and_types`. + * \returns `RMW_RET_OK` if successful, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `topic_endpoint_info_array` is NULL, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `topic_endpoint_info_array` is not + * a zero initialized array, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `allocator` is invalid, + * by rcutils_allocator_is_valid() definition, or + * \returns `RMW_BAD_ALLOC` if memory allocation fails, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + * \remark This function sets the RMW error state on failure. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_array_init_with_size( + rmw_topic_endpoint_info_array_t * topic_endpoint_info_array, + size_t size, + rcutils_allocator_t * allocator); + +/// Finalize an array of topic endpoint information. +/** + * This function deallocates the given array storage, and then zero initializes it. + * If a logical error, such as `RMW_RET_INVALID_ARGUMENT`, ensues, this function will + * return early, leaving the given array unchanged. + * Otherwise, it will proceed despite errors. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \par Thread-safety + * Finalization is a reentrant procedure, but: + * - Access to the array of topic endpoint information is not synchronized. + * It is not safe to read or write `topic_endpoint_info_array` during finalization. + * - The default allocators are thread-safe objects, but any custom `allocator` may not be. + * Check your allocator documentation for further reference. + * + * \pre Given `allocator` must be the same used to initialize the given `topic_endpoint_info_array`. + * + * \param[inout] topic_endpoint_info_array object to be finalized. + * \param[in] allocator Allocator used to populate the given `topic_endpoint_info_array`. + * \returns `RMW_RET_OK` if successful, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `topic_endpoint_info_array` is NULL, or + * \returns `RMW_RET_INVALID_ARGUMENT` if `allocator` is invalid, + * by rcutils_allocator_is_valid() definition, or + * \returns `RMW_RET_ERROR` when an unspecified error occurs. + * \remark This function sets the RMW error state on failure. + */ +RMW_PUBLIC +RMW_WARN_UNUSED +rmw_ret_t +rmw_topic_endpoint_info_array_fini( + rmw_topic_endpoint_info_array_t * topic_endpoint_info_array, + rcutils_allocator_t * allocator); + +#ifdef __cplusplus +} +#endif + +#endif // RMW_DDS_COMMON__TOPIC_ENDPOINT_INFO_ARRAY_H_ diff --git a/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/visibility_control.h b/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/visibility_control.h new file mode 100644 index 00000000..9d51c6d2 --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/include/rmw_dds_common/visibility_control.h @@ -0,0 +1,58 @@ +// 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 RMW_DDS_COMMON__VISIBILITY_CONTROL_H_ +#define RMW_DDS_COMMON__VISIBILITY_CONTROL_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RMW_DDS_COMMON_EXPORT __attribute__ ((dllexport)) + #define RMW_DDS_COMMON_IMPORT __attribute__ ((dllimport)) + #else + #define RMW_DDS_COMMON_EXPORT __declspec(dllexport) + #define RMW_DDS_COMMON_IMPORT __declspec(dllimport) + #endif + #ifdef RMW_DDS_COMMON_BUILDING_LIBRARY + #define RMW_DDS_COMMON_PUBLIC RMW_DDS_COMMON_EXPORT + #else + #define RMW_DDS_COMMON_PUBLIC RMW_DDS_COMMON_IMPORT + #endif + #define RMW_DDS_COMMON_PUBLIC_TYPE RMW_DDS_COMMON_PUBLIC + #define RMW_DDS_COMMON_LOCAL +#else + #define RMW_DDS_COMMON_EXPORT __attribute__ ((visibility("default"))) + #define RMW_DDS_COMMON_IMPORT + #if __GNUC__ >= 4 + #define RMW_DDS_COMMON_PUBLIC __attribute__ ((visibility("default"))) + #define RMW_DDS_COMMON_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RMW_DDS_COMMON_PUBLIC + #define RMW_DDS_COMMON_LOCAL + #endif + #define RMW_DDS_COMMON_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // RMW_DDS_COMMON__VISIBILITY_CONTROL_H_ diff --git a/rmw_dds_common/rmw_dds_common/msg/Gid.msg b/rmw_dds_common/rmw_dds_common/msg/Gid.msg new file mode 100644 index 00000000..97d1a4c0 --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/msg/Gid.msg @@ -0,0 +1 @@ +char[24] data diff --git a/rmw_dds_common/rmw_dds_common/msg/NodeEntitiesInfo.msg b/rmw_dds_common/rmw_dds_common/msg/NodeEntitiesInfo.msg new file mode 100644 index 00000000..00c410fc --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/msg/NodeEntitiesInfo.msg @@ -0,0 +1,4 @@ +string<=256 node_namespace +string<=256 node_name +Gid[] reader_gid_seq +Gid[] writer_gid_seq diff --git a/rmw_dds_common/rmw_dds_common/msg/ParticipantEntitiesInfo.msg b/rmw_dds_common/rmw_dds_common/msg/ParticipantEntitiesInfo.msg new file mode 100644 index 00000000..968b2530 --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/msg/ParticipantEntitiesInfo.msg @@ -0,0 +1,2 @@ +Gid gid +NodeEntitiesInfo[] node_entities_info_seq diff --git a/rmw_dds_common/rmw_dds_common/package.xml b/rmw_dds_common/rmw_dds_common/package.xml new file mode 100644 index 00000000..ab109030 --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/package.xml @@ -0,0 +1,32 @@ + + + + rmw_dds_common + 1.0.2 + Define a common interface between DDS implementations of ROS middleware. + Ivan Paunovic + Apache License 2.0 + Ivan Paunovic + + ament_cmake + rosidl_default_generators + + rosidl_default_runtime + + rcutils + rcpputils + rmw + rosidl_runtime_cpp + + ament_cmake_gmock + ament_lint_auto + ament_lint_common + + performance_test_fixture + + rosidl_interface_packages + + + ament_cmake + + diff --git a/rmw_connextdds_common/src/rmw_dds_common/gid_utils.cpp b/rmw_dds_common/rmw_dds_common/src/gid_utils.cpp similarity index 89% rename from rmw_connextdds_common/src/rmw_dds_common/gid_utils.cpp rename to rmw_dds_common/rmw_dds_common/src/gid_utils.cpp index 50ca3bdf..4e6a516e 100644 --- a/rmw_connextdds_common/src/rmw_dds_common/gid_utils.cpp +++ b/rmw_dds_common/rmw_dds_common/src/gid_utils.cpp @@ -12,18 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rmw_connextdds/static_config.hpp" - -#if !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON - #include #include #include #include "rmw/types.h" -#include "rmw_connextdds/gid_utils.hpp" -#include "rmw_connextdds_common/msg/gid.hpp" +#include "rmw_dds_common/gid_utils.hpp" +#include "rmw_dds_common/msg/gid.hpp" using rmw_dds_common::Compare_rmw_gid_t; @@ -77,5 +73,3 @@ rmw_dds_common::convert_msg_to_gid( assert(nullptr != gid); std::memcpy(const_cast(gid->data), &msg_gid->data, RMW_GID_STORAGE_SIZE); } - -#endif /* !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON */ diff --git a/rmw_connextdds_common/src/rmw_dds_common/graph_cache.cpp b/rmw_dds_common/rmw_dds_common/src/graph_cache.cpp similarity index 97% rename from rmw_connextdds_common/src/rmw_dds_common/graph_cache.cpp rename to rmw_dds_common/rmw_dds_common/src/graph_cache.cpp index 0d5d6b13..0474bd58 100644 --- a/rmw_connextdds_common/src/rmw_dds_common/graph_cache.cpp +++ b/rmw_dds_common/rmw_dds_common/src/graph_cache.cpp @@ -12,11 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rmw_connextdds/static_config.hpp" - -#if !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON - -#include "rmw_connextdds/graph_cache_common.hpp" +#include "rmw_dds_common/graph_cache.hpp" #include #include @@ -39,10 +35,10 @@ #include "rmw/convert_rcutils_ret_to_rmw_ret.h" #include "rmw/error_handling.h" #include "rmw/sanity_checks.h" -#include "rmw_connextdds/topic_endpoint_info.h" -#include "rmw_connextdds/topic_endpoint_info_array.h" +#include "rmw_dds_common/topic_endpoint_info.h" +#include "rmw_dds_common/topic_endpoint_info_array.h" -#include "rmw_connextdds/gid_utils.hpp" +#include "rmw_dds_common/gid_utils.hpp" using rmw_dds_common::GraphCache; using rmw_dds_common::operator<<; @@ -833,7 +829,7 @@ __get_names_and_types_by_node( namespace_); if (nullptr == node_info_ptr) { - return RMW_RET_NODE_NAME_NON_EXISTENT; + return /* RMW_RET_NODE_NAME_NON_EXISTENT */ RMW_RET_ERROR; } NamesAndTypes topics = __get_names_and_types_from_gids( @@ -1014,14 +1010,6 @@ GraphCache::get_node_names( "rmw_dds_common", "failed to cleanup during error handling: %s", rcutils_get_error_string().str); } - if (enclaves) { - rcutils_ret = rcutils_string_array_fini(enclaves); - if (rcutils_ret != RCUTILS_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_dds_common", - "failed to cleanup during error handling: %s", rcutils_get_error_string().str); - } - } return RMW_RET_BAD_ALLOC; } @@ -1071,5 +1059,3 @@ rmw_dds_common::operator<<(std::ostream & ostream, const GraphCache & graph_cach return ostream << ss.str(); } - -#endif /* !RMW_CONNEXT_HAVE_PKG_RMW_DDS_COMMON */ diff --git a/rmw_dds_common/rmw_dds_common/test/allocator_testing_utils.h b/rmw_dds_common/rmw_dds_common/test/allocator_testing_utils.h new file mode 100644 index 00000000..786b2f79 --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/test/allocator_testing_utils.h @@ -0,0 +1,94 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ALLOCATOR_TESTING_UTILS_H_ +#define ALLOCATOR_TESTING_UTILS_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +#include "rcutils/allocator.h" + +typedef struct __failing_allocator_state +{ + bool is_failing; +} __failing_allocator_state; + +void * +failing_malloc(size_t size, void * state) +{ + if (((__failing_allocator_state *)state)->is_failing) { + return nullptr; + } + return rcutils_get_default_allocator().allocate(size, rcutils_get_default_allocator().state); +} + +void * +failing_realloc(void * pointer, size_t size, void * state) +{ + if (((__failing_allocator_state *)state)->is_failing) { + return nullptr; + } + return rcutils_get_default_allocator().reallocate( + pointer, size, rcutils_get_default_allocator().state); +} + +void +failing_free(void * pointer, void * state) +{ + if (((__failing_allocator_state *)state)->is_failing) { + return; + } + rcutils_get_default_allocator().deallocate(pointer, rcutils_get_default_allocator().state); +} + +void * +failing_calloc(size_t number_of_elements, size_t size_of_element, void * state) +{ + if (((__failing_allocator_state *)state)->is_failing) { + return nullptr; + } + return rcutils_get_default_allocator().zero_allocate( + number_of_elements, size_of_element, rcutils_get_default_allocator().state); +} + +static inline rcutils_allocator_t +get_failing_allocator(void) +{ + static __failing_allocator_state state; + state.is_failing = true; + auto failing_allocator = rcutils_get_default_allocator(); + failing_allocator.allocate = failing_malloc; + failing_allocator.deallocate = failing_free; + failing_allocator.reallocate = failing_realloc; + failing_allocator.zero_allocate = failing_calloc; + failing_allocator.state = &state; + return failing_allocator; +} + +static inline void +set_failing_allocator_is_failing(rcutils_allocator_t & failing_allocator, bool state) +{ + ((__failing_allocator_state *)failing_allocator.state)->is_failing = state; +} + +#ifdef __cplusplus +} +#endif + +#endif // ALLOCATOR_TESTING_UTILS_H_ diff --git a/rmw_dds_common/rmw_dds_common/test/benchmark/benchmark_graph_cache.cpp b/rmw_dds_common/rmw_dds_common/test/benchmark/benchmark_graph_cache.cpp new file mode 100644 index 00000000..df02824a --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/test/benchmark/benchmark_graph_cache.cpp @@ -0,0 +1,405 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "performance_test_fixture/performance_test_fixture.hpp" + +#include "rmw/qos_profiles.h" + +#include "rmw_dds_common/gid_utils.hpp" +#include "rmw_dds_common/graph_cache.hpp" + +using performance_test_fixture::PerformanceTest; +using rmw_dds_common::GraphCache; + +std::string +identity_demangle(const std::string & name) +{ + return name; +} + +rmw_gid_t +gid_from_string(const std::string & str) +{ + rmw_gid_t gid = {}; + memcpy( + reinterpret_cast(gid.data), + str.c_str(), + str.size() + 1); + return gid; +} + +void +add_participants( + GraphCache & graph_cache, + const std::vector & gids) +{ + for (const auto & gid : gids) { + graph_cache.add_participant(gid_from_string(gid), ""); + } +} + +void +remove_participants( + GraphCache & graph_cache, + const std::vector & gids) +{ + for (const auto & gid : gids) { + graph_cache.remove_participant(gid_from_string(gid)); + } +} + +struct NodeInfo +{ + std::string gid; + std::string namespace_; + std::string name; +}; + +rmw_dds_common::msg::ParticipantEntitiesInfo +add_nodes( + GraphCache & graph_cache, + const std::vector & node_info) +{ + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + for (const auto & elem : node_info) { + msg = graph_cache.add_node( + gid_from_string(elem.gid), + elem.name, + elem.namespace_); + } + return msg; +} + +rmw_dds_common::msg::ParticipantEntitiesInfo +remove_nodes( + GraphCache & graph_cache, + const std::vector & node_info) +{ + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + for (const auto & elem : node_info) { + msg = graph_cache.remove_node( + gid_from_string(elem.gid), + elem.name, + elem.namespace_); + } + return msg; +} + +struct EntityInfo +{ + std::string gid; + std::string participant_gid; + std::string name; + std::string type; + bool is_reader; +}; + +void +add_entities( + GraphCache & graph_cache, + const std::vector & entities_info) +{ + for (const auto & elem : entities_info) { + graph_cache.add_entity( + gid_from_string(elem.gid), + elem.name, + elem.type, + gid_from_string(elem.participant_gid), + rmw_qos_profile_default, + elem.is_reader); + } +} + +void +remove_entities( + GraphCache & graph_cache, + const std::vector & entities_info) +{ + for (const auto & elem : entities_info) { + graph_cache.remove_entity( + gid_from_string(elem.gid), + elem.is_reader); + } +} + + +struct EntityAssociations +{ + std::string gid; + bool is_reader; + std::string participant_gid; + std::string namespace_; + std::string name; +}; + +void associate_entities( + GraphCache & graph_cache, + const std::vector & associations) +{ + for (const auto & elem : associations) { + if (elem.is_reader) { + graph_cache.associate_reader( + gid_from_string(elem.gid), + gid_from_string(elem.participant_gid), + elem.name, + elem.namespace_); + } else { + graph_cache.associate_writer( + gid_from_string(elem.gid), + gid_from_string(elem.participant_gid), + elem.name, + elem.namespace_); + } + } +} + +void dissociate_entities( + GraphCache & graph_cache, + const std::vector & associations) +{ + for (const auto & elem : associations) { + if (elem.is_reader) { + graph_cache.dissociate_reader( + gid_from_string(elem.gid), + gid_from_string(elem.participant_gid), + elem.name, + elem.namespace_); + } else { + graph_cache.dissociate_writer( + gid_from_string(elem.gid), + gid_from_string(elem.participant_gid), + elem.name, + elem.namespace_); + } + } +} + +class TestGraphCache : public PerformanceTest +{ +public: + void SetUp(benchmark::State & st) + { + add_participants(graph_cache, {"participant1"}); + add_entities( + graph_cache, + { + // topic1 + {"reader1", "participant1", "topic1", "Str", true}, + {"writer1", "participant1", "topic1", "Str", false}, + }); + add_nodes( + graph_cache, { + {"participant1", "ns1", "node1"}, + {"participant1", "ns1", "node2"}, + {"participant1", "ns2", "node1"}}); + performance_test_fixture::PerformanceTest::SetUp(st); + } + void TearDown(::benchmark::State & st) + { + performance_test_fixture::PerformanceTest::TearDown(st); + remove_nodes( + graph_cache, { + {"participant1", "ns1", "node1"}, + {"participant1", "ns1", "node2"}, + {"participant1", "ns2", "node1"}}); + remove_entities( + graph_cache, + { + // topic1 + {"reader1", "participant1", "topic1", "Str", true}, + {"writer1", "participant1", "topic1", "Str", false}, + }); + remove_participants(graph_cache, {"participant1"}); + } + +protected: + GraphCache graph_cache; +}; + +BENCHMARK_F(PerformanceTest, add_remove_participant_benchmark)(benchmark::State & st) +{ + GraphCache graph_cache; + + for (auto _ : st) { + add_participants(graph_cache, {"participant1"}); + remove_participants(graph_cache, {"participant1"}); + } +} + +BENCHMARK_F(PerformanceTest, add_remove_participant_and_node_benchmark)(benchmark::State & st) +{ + GraphCache graph_cache; + + reset_heap_counters(); + + for (auto _ : st) { + add_participants(graph_cache, {"participant1"}); + add_nodes(graph_cache, {{"participant1", "ns1", "node"}}); + remove_nodes(graph_cache, {{"participant1", "ns1", "node"}}); + remove_participants(graph_cache, {"participant1"}); + } +} + +BENCHMARK_F(TestGraphCache, get_writers_info_by_topic_benchmark)(benchmark::State & st) +{ + rmw_topic_endpoint_info_array_t info = rmw_get_zero_initialized_topic_endpoint_info_array(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + for (auto _ : st) { + rmw_ret_t ret = graph_cache.get_writers_info_by_topic( + "topic1", + identity_demangle, + &allocator, + &info); + if (ret != RMW_RET_OK) { + st.SkipWithError("get_writers_info_by_topic failed"); + } + ret = rmw_topic_endpoint_info_array_fini(&info, &allocator); + if (ret != RMW_RET_OK) { + st.SkipWithError("rmw_topic_endpoint_info_array_fini failed"); + } + } +} + +BENCHMARK_F(TestGraphCache, get_readers_info_by_topic_benchmark)(benchmark::State & st) +{ + rmw_topic_endpoint_info_array_t info = rmw_get_zero_initialized_topic_endpoint_info_array(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + for (auto _ : st) { + rmw_ret_t ret = graph_cache.get_readers_info_by_topic( + "topic1", + identity_demangle, + &allocator, + &info); + if (ret != RMW_RET_OK) { + st.SkipWithError("get_readers_info_by_topic failed"); + } + ret = rmw_topic_endpoint_info_array_fini(&info, &allocator); + if (ret != RMW_RET_OK) { + st.SkipWithError("rmw_topic_endpoint_info_array_fini failed"); + } + } +} + +BENCHMARK_F(TestGraphCache, get_names_and_types_benchmark)(benchmark::State & st) +{ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + for (auto _ : st) { + rmw_names_and_types_t names_and_types = rmw_get_zero_initialized_names_and_types(); + rmw_ret_t ret = graph_cache.get_names_and_types( + identity_demangle, + identity_demangle, + &allocator, + &names_and_types); + if (ret != RMW_RET_OK) { + st.SkipWithError("get_names_and_types failed"); + } + ret = rmw_names_and_types_fini(&names_and_types); + if (ret != RMW_RET_OK) { + st.SkipWithError("rmw_names_and_types_fini failed"); + } + } +} + +BENCHMARK_F(TestGraphCache, get_reader_names_and_types_by_node)(benchmark::State & st) +{ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + for (auto _ : st) { + rmw_names_and_types_t names_and_types = rmw_get_zero_initialized_names_and_types(); + rmw_ret_t ret = graph_cache.get_reader_names_and_types_by_node( + "node1", + "ns1", + identity_demangle, + identity_demangle, + &allocator, + &names_and_types); + if (ret != RMW_RET_OK) { + st.SkipWithError("get_reader_names_and_types_by_node failed"); + } + ret = rmw_names_and_types_fini(&names_and_types); + if (ret != RMW_RET_OK) { + st.SkipWithError("rmw_names_and_types_fini failed"); + } + } +} + +BENCHMARK_F(TestGraphCache, get_writer_names_and_types_by_node)(benchmark::State & st) +{ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + for (auto _ : st) { + rmw_names_and_types_t names_and_types = rmw_get_zero_initialized_names_and_types(); + rmw_ret_t ret = graph_cache.get_writer_names_and_types_by_node( + "node1", + "ns1", + identity_demangle, + identity_demangle, + &allocator, + &names_and_types); + if (ret != RMW_RET_OK) { + st.SkipWithError("get_writer_names_and_types_by_node failed"); + } + ret = rmw_names_and_types_fini(&names_and_types); + if (ret != RMW_RET_OK) { + st.SkipWithError("rmw_names_and_types_fini failed"); + } + } +} + +BENCHMARK_F(TestGraphCache, get_reader_count_benchmark)(benchmark::State & st) +{ + size_t count; + for (auto _ : st) { + rmw_ret_t ret = graph_cache.get_reader_count("reader1", &count); + if (ret != RMW_RET_OK) { + st.SkipWithError("get_reader_count failed"); + } + } +} + +BENCHMARK_F(TestGraphCache, get_writer_count_benchmark)(benchmark::State & st) +{ + size_t count; + for (auto _ : st) { + rmw_ret_t ret = graph_cache.get_writer_count("writer1", &count); + if (ret != RMW_RET_OK) { + st.SkipWithError("get_reader_count failed"); + } + } +} + +BENCHMARK_F(TestGraphCache, associate_entities_benchmark)(benchmark::State & st) +{ + for (auto _ : st) { + // Associate entities + associate_entities( + graph_cache, + { + {"reader1", true, "participant1", "ns1", "node1"}, + {"writer1", false, "participant1", "ns1", "node2"}, + }); + dissociate_entities( + graph_cache, + { + {"reader1", true, "participant1", "ns1", "node1"}, + {"writer1", false, "participant1", "ns1", "node2"}, + }); + } +} diff --git a/rmw_dds_common/rmw_dds_common/test/test_gid_utils.cpp b/rmw_dds_common/rmw_dds_common/test/test_gid_utils.cpp new file mode 100644 index 00000000..92b237bd --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/test/test_gid_utils.cpp @@ -0,0 +1,38 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rmw_dds_common/gid_utils.hpp" + +using rmw_dds_common::Compare_rmw_gid_t; +using rmw_dds_common::operator==; +using rmw_dds_common::operator<<; + +TEST(test_gid_utils, test_gid_operators) +{ + rmw_gid_t gid_1 = {0, {0}}; + rmw_gid_t gid_2 = {0, {0}}; + + struct Compare_rmw_gid_t compare; + EXPECT_FALSE(compare(gid_1, gid_2)); + + EXPECT_TRUE(gid_1 == gid_2); + + rmw_gid_t gid_3 = {"foo", {0}}; + + std::ostringstream stream; + stream << gid_3; + ASSERT_STREQ(stream.str().c_str(), "0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0"); +} diff --git a/rmw_dds_common/rmw_dds_common/test/test_graph_cache.cpp b/rmw_dds_common/rmw_dds_common/test/test_graph_cache.cpp new file mode 100644 index 00000000..c856d0de --- /dev/null +++ b/rmw_dds_common/rmw_dds_common/test/test_graph_cache.cpp @@ -0,0 +1,1651 @@ +// 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 +#include +#include +#include + +#include "osrf_testing_tools_cpp/scope_exit.hpp" + +#include "rcutils/testing/fault_injection.h" +#include "rmw/qos_profiles.h" +#include "rmw/topic_endpoint_info.h" +#include "rmw/topic_endpoint_info_array.h" +#include "./allocator_testing_utils.h" + +#include "rmw_dds_common/gid_utils.hpp" +#include "rmw_dds_common/graph_cache.hpp" + +using rmw_dds_common::GraphCache; +using rmw_dds_common::operator==; + +struct NameAndNamespace +{ + std::string namespace_; + std::string name; +}; + +MATCHER_P2(IsNameAndNamespace, namespace_, name, "") +{ + return (arg.namespace_ == namespace_) && (arg.name == name); +} + +void +check_names_and_namespace( + const rcutils_string_array_t & names, + const rcutils_string_array_t & namespaces, + const std::vector & expected) +{ + ASSERT_EQ(names.size, namespaces.size); + ASSERT_EQ(expected.size(), names.size); + for (size_t i = 0; i < expected.size(); i++) { + ASSERT_THAT( + expected, + testing::Contains(IsNameAndNamespace(namespaces.data[i], names.data[i]))); + } +} + +struct NameAndTypes +{ + std::string name; + std::vector types; +}; + +void check_names_and_types( + const rmw_names_and_types_t & names_and_types, + const std::vector & expected) +{ + ASSERT_EQ(names_and_types.names.size, expected.size()); + + for (size_t i = 0; i < expected.size(); i++) { + const auto & item = expected[i]; + EXPECT_EQ(item.name, names_and_types.names.data[i]); + const auto & expected_types = item.types; + const auto & types = names_and_types.types[i]; + ASSERT_EQ(expected_types.size(), types.size); + for (size_t j = 0; j < expected_types.size(); j++) { + EXPECT_EQ(expected_types[j], types.data[j]); + } + } +} + +std::string +identity_demangle(const std::string & name) +{ + return name; +} + +using DemangleFunctionT = GraphCache::DemangleFunctionT; + +void +check_results( + const GraphCache & graph_cache, + const std::vector & nodes_names_and_namespaces = {}, + const std::vector & topics_names_and_types = {}, + DemangleFunctionT demangle_topic = identity_demangle, + DemangleFunctionT demangle_type = identity_demangle) +{ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + { + rcutils_string_array_t names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t namespaces = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t enclaves = rcutils_get_zero_initialized_string_array(); + graph_cache.get_node_names(&names, &namespaces, &enclaves, &allocator); + check_names_and_namespace(names, namespaces, nodes_names_and_namespaces); + } + + EXPECT_EQ(nodes_names_and_namespaces.size(), graph_cache.get_number_of_nodes()); + + { + rmw_names_and_types_t names_and_types = rmw_get_zero_initialized_names_and_types(); + graph_cache.get_names_and_types( + demangle_topic, + demangle_type, + &allocator, + &names_and_types); + check_names_and_types(names_and_types, topics_names_and_types); + } +} + +void check_results_by_node( + const GraphCache & graph_cache, + const std::string & node_namespace, + const std::string & node_name, + const std::vector & readers_names_and_types = {}, + const std::vector & writers_names_and_types = {}, + DemangleFunctionT demangle_topic = identity_demangle, + DemangleFunctionT demangle_type = identity_demangle) +{ + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + { + rmw_names_and_types_t names_and_types = rmw_get_zero_initialized_names_and_types(); + graph_cache.get_reader_names_and_types_by_node( + node_name, + node_namespace, + demangle_topic, + demangle_type, + &allocator, + &names_and_types); + check_names_and_types(names_and_types, readers_names_and_types); + } + + { + rmw_names_and_types_t names_and_types = rmw_get_zero_initialized_names_and_types(); + graph_cache.get_writer_names_and_types_by_node( + node_name, + node_namespace, + demangle_topic, + demangle_type, + &allocator, + &names_and_types); + check_names_and_types(names_and_types, writers_names_and_types); + } +} + + +void +check_results_by_topic( + const GraphCache & graph_cache, + const std::string topic_name, + size_t readers_count = 0U, + size_t writers_count = 0U) +{ + { + // Start with a value different from the expected one, to + // ensure that the function is actually doing something. + size_t count = readers_count + 1; + EXPECT_EQ( + graph_cache.get_reader_count(topic_name, &count), + RMW_RET_OK); + EXPECT_EQ(readers_count, count); + } + + { + size_t count = writers_count + 1; + EXPECT_EQ( + graph_cache.get_writer_count(topic_name, &count), + RMW_RET_OK); + EXPECT_EQ(writers_count, count); + } +} + +rmw_gid_t +gid_from_string(const std::string & str) +{ + rmw_gid_t gid = {}; + EXPECT_LT(str.size(), RMW_GID_STORAGE_SIZE); + memcpy( + reinterpret_cast(gid.data), + str.c_str(), + str.size() + 1); + return gid; +} + +struct EndpointInfo +{ + std::string gid; + std::string node_namespace; + std::string node_name; + std::string topic_type; +}; + +void +check_results_by_topic( + const GraphCache & graph_cache, + const std::string topic_name, + const std::vector & readers_info, + const std::vector & writers_info) +{ + { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_topic_endpoint_info_array_t info = + rmw_get_zero_initialized_topic_endpoint_info_array(); + ASSERT_EQ( + graph_cache.get_readers_info_by_topic( + topic_name, + identity_demangle, + &allocator, + &info), + RMW_RET_OK); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(rmw_topic_endpoint_info_array_fini(&info, &allocator), RMW_RET_OK); + }); + ASSERT_EQ(info.size, readers_info.size()); + for (size_t i = 0; i < readers_info.size(); ++i) { + rmw_gid_t endpoint_gid = {}; + memcpy(endpoint_gid.data, info.info_array[i].endpoint_gid, RMW_GID_STORAGE_SIZE); + EXPECT_EQ(gid_from_string(readers_info[i].gid), endpoint_gid); + EXPECT_EQ(readers_info[i].node_name, info.info_array[i].node_name); + EXPECT_EQ(readers_info[i].node_namespace, info.info_array[i].node_namespace); + EXPECT_EQ(readers_info[i].topic_type, info.info_array[i].topic_type); + EXPECT_EQ(RMW_ENDPOINT_SUBSCRIPTION, info.info_array[i].endpoint_type); + } + } + + { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_topic_endpoint_info_array_t info = + rmw_get_zero_initialized_topic_endpoint_info_array(); + ASSERT_EQ( + graph_cache.get_writers_info_by_topic( + topic_name, + identity_demangle, + &allocator, + &info), + RMW_RET_OK); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(rmw_topic_endpoint_info_array_fini(&info, &allocator), RMW_RET_OK); + }); + ASSERT_EQ(info.size, writers_info.size()); + for (size_t i = 0; i < writers_info.size(); ++i) { + rmw_gid_t endpoint_gid = {}; + memcpy(endpoint_gid.data, info.info_array[i].endpoint_gid, RMW_GID_STORAGE_SIZE); + EXPECT_EQ(gid_from_string(writers_info[i].gid), endpoint_gid); + EXPECT_EQ(writers_info[i].node_name, info.info_array[i].node_name); + EXPECT_EQ(writers_info[i].node_namespace, info.info_array[i].node_namespace); + EXPECT_EQ(writers_info[i].topic_type, info.info_array[i].topic_type); + EXPECT_EQ(RMW_ENDPOINT_PUBLISHER, info.info_array[i].endpoint_type); + } + } +} + +TEST(test_graph_cache, zero_initialized) +{ + GraphCache graph_cache; + + check_results(graph_cache); + check_results_by_node(graph_cache, "some_namespace", "node/name"); + check_results_by_topic(graph_cache, "some/topic/name"); +} + +struct EntityInfo +{ + std::string gid; + std::string participant_gid; + std::string name; + std::string type; + bool is_reader; +}; + +void +add_entities( + GraphCache & graph_cache, + const std::vector & entities_info) +{ + for (const auto & elem : entities_info) { + EXPECT_TRUE( + graph_cache.add_entity( + gid_from_string(elem.gid), + elem.name, + elem.type, + gid_from_string(elem.participant_gid), + rmw_qos_profile_default, + elem.is_reader)); + } +} + +void +remove_entities( + GraphCache & graph_cache, + const std::vector & entities_info) +{ + for (const auto & elem : entities_info) { + EXPECT_TRUE( + graph_cache.remove_entity( + gid_from_string(elem.gid), + elem.is_reader)); + } +} + + +TEST(test_graph_cache, add_remove_entities) +{ + GraphCache graph_cache; + + // Add some readers. + add_entities( + graph_cache, + { + // topic1 readers + {"reader1", "participant1", "topic1", "Str", true}, + {"reader2", "participant1", "topic1", "Str", true}, + {"reader3", "participant1", "topic1", "Str", true}, + {"reader4", "participant1", "topic1", "Str", true}, + // topic2 readers + {"reader5", "participant1", "topic2", "Str", true}, + {"reader6", "participant1", "topic2", "Int", true}, + // topic3 readers + {"reader7", "participant1", "topic3", "Float", true}, + }); + + // Check graph state. + check_results( + graph_cache, + {}, + { + {"topic1", {"Str"}}, + {"topic2", {"Int", "Str"}}, + {"topic3", {"Float"}}, + }); + check_results_by_node(graph_cache, "ns", "name"); + check_results_by_topic(graph_cache, "topic1", 4); + check_results_by_topic(graph_cache, "topic2", 2); + check_results_by_topic(graph_cache, "topic3", 1); + + // Add some writers. + add_entities( + graph_cache, + { + // topic1 writers + {"writer1", "participant1", "topic1", "Str", false}, + {"writer2", "participant1", "topic1", "Str", false}, + // topic2 writers + {"writer5", "participant1", "topic2", "Str", false}, + {"writer6", "participant1", "topic2", "Float", false}, + {"writer7", "participant1", "topic2", "Bool", false}, + // topic4 writers + {"writer8", "participant1", "topic4", "Int", false}, + }); + + // Check graph state. + check_results( + graph_cache, + {}, + { + {"topic1", {"Str"}}, + {"topic2", {"Bool", "Float", "Int", "Str"}}, + {"topic3", {"Float"}}, + {"topic4", {"Int"}}, + }); + check_results_by_node(graph_cache, "ns", "name"); + check_results_by_topic(graph_cache, "topic1", 4, 2); + check_results_by_topic(graph_cache, "topic2", 2, 3); + check_results_by_topic(graph_cache, "topic3", 1, 0); + check_results_by_topic(graph_cache, "topic4", 0, 1); + + // Remove some readers and writers. + remove_entities( + graph_cache, + { + // topic1 + {"reader2", "participant1", "topic1", "Str", true}, + {"reader3", "participant1", "topic1", "Str", true}, + {"reader4", "participant1", "topic1", "Str", true}, + {"writer2", "participant1", "topic1", "Str", false}, + // topic2 + {"reader6", "participant1", "topic2", "Int", true}, + {"writer5", "participant1", "topic2", "Str", false}, + {"writer6", "participant1", "topic2", "Float", false}, + {"writer7", "participant1", "topic2", "Bool", false}, + // topic3 + {"reader7", "participant1", "topic3", "Float", true}, + }); + + // Check graph state. + check_results( + graph_cache, + {}, + { + {"topic1", {"Str"}}, + {"topic2", {"Str"}}, + {"topic4", {"Int"}}, + }); + check_results_by_node(graph_cache, "ns", "name"); + check_results_by_topic(graph_cache, "topic1", 1, 1); + check_results_by_topic(graph_cache, "topic2", 1, 0); + check_results_by_topic(graph_cache, "topic3", 0, 0); + check_results_by_topic(graph_cache, "topic4", 0, 1); + + // Remove the remaining readers and writers. + remove_entities( + graph_cache, + { + // topic1 + {"reader1", "participant1", "topic1", "Str", true}, + {"writer1", "participant1", "topic1", "Str", false}, + // topic2 + {"reader5", "participant1", "topic2", "Str", true}, + // topic4 + {"writer8", "participant1", "topic4", "Int", false}, + }); + + // Check graph state. + check_results(graph_cache); + check_results_by_node(graph_cache, "ns", "name"); + check_results_by_topic(graph_cache, "topic1"); + check_results_by_topic(graph_cache, "topic2"); + check_results_by_topic(graph_cache, "topic3"); + check_results_by_topic(graph_cache, "topic4"); +} + +void +add_participants( + GraphCache & graph_cache, + const std::vector & gids) +{ + for (const auto & gid : gids) { + graph_cache.add_participant(gid_from_string(gid), ""); + } +} + +void +remove_participants( + GraphCache & graph_cache, + const std::vector & gids) +{ + for (const auto & gid : gids) { + graph_cache.remove_participant(gid_from_string(gid)); + } +} + +rmw_dds_common::msg::Gid +gid_msg_from_string(const std::string & str) +{ + rmw_dds_common::msg::Gid gid; + gid.data = {}; + EXPECT_LT(str.size(), RMW_GID_STORAGE_SIZE); + memcpy( + reinterpret_cast(gid.data.data()), + str.c_str(), + str.size() + 1); + return gid; +} + +struct NodeInfo +{ + std::string gid; + std::string namespace_; + std::string name; +}; + +rmw_dds_common::msg::ParticipantEntitiesInfo +add_nodes( + GraphCache & graph_cache, + const std::vector & node_info) +{ + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + for (const auto & elem : node_info) { + msg = graph_cache.add_node( + gid_from_string(elem.gid), + elem.name, + elem.namespace_); + } + return msg; +} + +rmw_dds_common::msg::ParticipantEntitiesInfo +remove_nodes( + GraphCache & graph_cache, + const std::vector & node_info) +{ + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + for (const auto & elem : node_info) { + msg = graph_cache.remove_node( + gid_from_string(elem.gid), + elem.name, + elem.namespace_); + } + return msg; +} + +struct NodeEntitiesInfo +{ + std::string namespace_; + std::string name; + std::vector reader_gids; + std::vector writer_gids; +}; + +struct ParticipantEntitiesInfo +{ + std::string gid; + std::vector node_entities_info_seq; +}; + +void +check_participant_entities_msg( + const rmw_dds_common::msg::ParticipantEntitiesInfo & msg, + const ParticipantEntitiesInfo & expected) +{ + EXPECT_EQ(msg.gid, gid_msg_from_string(expected.gid)); + ASSERT_EQ(msg.node_entities_info_seq.size(), expected.node_entities_info_seq.size()); + for (size_t i = 0; i < expected.node_entities_info_seq.size(); i++) { + const auto & node_info = msg.node_entities_info_seq[i]; + const auto & node_info_expected = expected.node_entities_info_seq[i]; + EXPECT_EQ(node_info.node_namespace, node_info_expected.namespace_); + EXPECT_EQ(node_info.node_name, node_info_expected.name); + auto & readers_gids = node_info.reader_gid_seq; + auto & expected_readers_gids = node_info_expected.reader_gids; + ASSERT_EQ(readers_gids.size(), expected_readers_gids.size()); + for (size_t j = 0; j < readers_gids.size(); j++) { + EXPECT_EQ(readers_gids[j], gid_msg_from_string(expected_readers_gids[j])); + } + auto & writers_gids = node_info.writer_gid_seq; + auto & expected_writers_gids = node_info_expected.writer_gids; + ASSERT_EQ(writers_gids.size(), expected_writers_gids.size()); + for (size_t j = 0; j < writers_gids.size(); j++) { + EXPECT_EQ(writers_gids[j], gid_msg_from_string(expected_writers_gids[j])); + } + } +} + +struct EntityAssociations +{ + std::string gid; + bool is_reader; + std::string participant_gid; + std::string namespace_; + std::string name; +}; + +void associate_entities( + GraphCache & graph_cache, + const std::vector & associations) +{ + for (const auto & elem : associations) { + if (elem.is_reader) { + graph_cache.associate_reader( + gid_from_string(elem.gid), + gid_from_string(elem.participant_gid), + elem.name, + elem.namespace_); + } else { + graph_cache.associate_writer( + gid_from_string(elem.gid), + gid_from_string(elem.participant_gid), + elem.name, + elem.namespace_); + } + } +} + +void dissociate_entities( + GraphCache & graph_cache, + const std::vector & associations) +{ + for (const auto & elem : associations) { + if (elem.is_reader) { + graph_cache.dissociate_reader( + gid_from_string(elem.gid), + gid_from_string(elem.participant_gid), + elem.name, + elem.namespace_); + } else { + graph_cache.dissociate_writer( + gid_from_string(elem.gid), + gid_from_string(elem.participant_gid), + elem.name, + elem.namespace_); + } + } +} + +rmw_dds_common::msg::ParticipantEntitiesInfo +get_participant_entities_info_msg(const ParticipantEntitiesInfo & info) +{ + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + msg.gid = gid_msg_from_string(info.gid); + msg.node_entities_info_seq.reserve(info.node_entities_info_seq.size()); + for (const auto & elem : info.node_entities_info_seq) { + msg.node_entities_info_seq.emplace_back(); + auto & node_info = msg.node_entities_info_seq.back(); + node_info.node_namespace = elem.namespace_; + node_info.node_name = elem.name; + auto & readers_gids = elem.reader_gids; + node_info.reader_gid_seq.reserve(readers_gids.size()); + for (const auto & reader_gid : readers_gids) { + node_info.reader_gid_seq.emplace_back(gid_msg_from_string(reader_gid)); + } + auto & writers_gids = elem.writer_gids; + node_info.writer_gid_seq.reserve(writers_gids.size()); + for (const auto & writer_gid : writers_gids) { + node_info.writer_gid_seq.emplace_back(gid_msg_from_string(writer_gid)); + } + } + return msg; +} + +TEST(test_graph_cache, normal_usage) +{ + GraphCache graph_cache; + + bool change_callback_called = false; + auto change_callback = [&change_callback_called]() { + change_callback_called = true; + }; + graph_cache.set_on_change_callback(change_callback); + + // Add one participant. + change_callback_called = false; + add_participants(graph_cache, {"participant1"}); + EXPECT_TRUE(change_callback_called); + + // Check state. + check_results(graph_cache); + check_results_by_node(graph_cache, "ns", "some_random_node"); + check_results_by_topic(graph_cache, "some_topic"); + + // Add some nodes. + change_callback_called = false; + check_participant_entities_msg( + add_nodes( + graph_cache, { + {"participant1", "ns1", "node1"}, + {"participant1", "ns1", "node2"}, + {"participant1", "ns2", "node1"}}), + { + "participant1", { + {"ns1", "node1", {}, {}}, + {"ns1", "node2", {}, {}}, + {"ns2", "node1", {}, {}}, + } + }); + EXPECT_TRUE(change_callback_called); + + // Check state. + check_results( + graph_cache, + { + {"ns1", "node1"}, + {"ns1", "node2"}, + {"ns2", "node1"}, + }); + check_results_by_node(graph_cache, "ns1", "node1"); + check_results_by_node(graph_cache, "ns1", "node2"); + check_results_by_node(graph_cache, "ns2", "node1"); + check_results_by_node(graph_cache, "ns", "some_random_node"); + check_results_by_topic(graph_cache, "some_topic"); + + // Add more participants and nodes. + add_participants(graph_cache, {"participant2", "participant3"}); + check_participant_entities_msg( + add_nodes( + graph_cache, { + {"participant2", "ns1", "node3"}, + {"participant2", "ns3", "node1"}}), + { + "participant2", { + {"ns1", "node3", {}, {}}, + {"ns3", "node1", {}, {}}, + } + }); + + // Check state. + check_results( + graph_cache, + { + {"ns1", "node1"}, + {"ns1", "node2"}, + {"ns1", "node3"}, + {"ns2", "node1"}, + {"ns3", "node1"}, + }); + check_results_by_node(graph_cache, "ns1", "node1"); + check_results_by_node(graph_cache, "ns1", "node2"); + check_results_by_node(graph_cache, "ns1", "node3"); + check_results_by_node(graph_cache, "ns2", "node1"); + check_results_by_node(graph_cache, "ns3", "node1"); + check_results_by_node(graph_cache, "ns", "some_random_node"); + check_results_by_topic(graph_cache, "some_topic"); + + // Add some readers and writers. + change_callback_called = false; + add_entities( + graph_cache, + { + // topic1 + {"reader1", "participant1", "topic1", "Str", true}, + {"reader2", "participant1", "topic1", "Float", true}, + {"writer1", "participant2", "topic1", "Int", false}, + {"writer2", "participant2", "topic1", "Str", false}, + // topic2 + {"reader3", "participant1", "topic2", "Str", true}, + {"reader4", "participant1", "topic2", "Str", true}, + {"reader5", "participant2", "topic2", "Str", true}, + // topic3 + {"writer3", "participant1", "topic3", "Bool", false}, + }); + EXPECT_TRUE(change_callback_called); + + // Check state. + check_results( + graph_cache, + { + {"ns1", "node1"}, + {"ns1", "node2"}, + {"ns1", "node3"}, + {"ns2", "node1"}, + {"ns3", "node1"}, + }, + { + {"topic1", {"Float", "Int", "Str"}}, + {"topic2", {"Str"}}, + {"topic3", {"Bool"}}, + }); + check_results_by_node(graph_cache, "ns1", "node1"); + check_results_by_node(graph_cache, "ns1", "node2"); + check_results_by_node(graph_cache, "ns1", "node3"); + check_results_by_node(graph_cache, "ns2", "node1"); + check_results_by_node(graph_cache, "ns3", "node1"); + check_results_by_node(graph_cache, "ns", "some_random_node"); + check_results_by_topic(graph_cache, "topic1", 2, 2); + check_results_by_topic(graph_cache, "topic2", 3, 0); + check_results_by_topic(graph_cache, "topic3", 0, 1); + check_results_by_topic(graph_cache, "some_topic", 0, 0); + + // Associate entities + change_callback_called = false; + associate_entities( + graph_cache, + { + // participant1, ns1, node1 + {"reader1", true, "participant1", "ns1", "node1"}, + {"reader2", true, "participant1", "ns1", "node1"}, + {"reader4", true, "participant1", "ns1", "node1"}, + {"writer3", false, "participant1", "ns1", "node1"}, + // participant1, ns2, node1 + {"reader3", true, "participant1", "ns2", "node1"}, + // participant2, ns1, node3 + {"reader5", true, "participant2", "ns1", "node3"}, + {"writer1", false, "participant2", "ns1", "node3"}, + {"writer2", false, "participant2", "ns1", "node3"}, + }); + EXPECT_TRUE(change_callback_called); + + // Check state. + check_results( + graph_cache, + { + {"ns1", "node1"}, + {"ns1", "node2"}, + {"ns1", "node3"}, + {"ns2", "node1"}, + {"ns3", "node1"}, + }, + { + {"topic1", {"Float", "Int", "Str"}}, + {"topic2", {"Str"}}, + {"topic3", {"Bool"}}, + }); + check_results_by_node( + graph_cache, "ns1", "node1", + { + {"topic1", {"Float", "Str"}}, + {"topic2", {"Str"}}, + }, + { + {"topic3", {"Bool"}}, + }); + check_results_by_node(graph_cache, "ns1", "node2"); + check_results_by_node( + graph_cache, "ns1", "node3", + { + {"topic2", {"Str"}}, + }, + { + {"topic1", {"Int", "Str"}}, + }); + check_results_by_node( + graph_cache, "ns2", "node1", + { + {"topic2", {"Str"}}, + }); + check_results_by_node(graph_cache, "ns3", "node1"); + check_results_by_node(graph_cache, "ns", "some_random_node"); + check_results_by_topic( + graph_cache, "topic1", + { + {"reader1", "ns1", "node1", "Str"}, + {"reader2", "ns1", "node1", "Float"}, + }, + { + {"writer1", "ns1", "node3", "Int"}, + {"writer2", "ns1", "node3", "Str"}, + }); + check_results_by_topic(graph_cache, "topic2", 3, 0); + check_results_by_topic(graph_cache, "topic3", 0, 1); + check_results_by_topic(graph_cache, "some_topic", 0, 0); + + // Associate entities + change_callback_called = false; + dissociate_entities( + graph_cache, + { + // participant1, ns1, node1 + {"reader1", true, "participant1", "ns1", "node1"}, + {"reader2", true, "participant1", "ns1", "node1"}, + // participant2, ns1, node3 + {"reader5", true, "participant2", "ns1", "node3"}, + {"writer1", false, "participant2", "ns1", "node3"}, + {"writer2", false, "participant2", "ns1", "node3"}, + }); + EXPECT_TRUE(change_callback_called); + + // Check state. + check_results( + graph_cache, + { + {"ns1", "node1"}, + {"ns1", "node2"}, + {"ns1", "node3"}, + {"ns2", "node1"}, + {"ns3", "node1"}, + }, + { + {"topic1", {"Float", "Int", "Str"}}, + {"topic2", {"Str"}}, + {"topic3", {"Bool"}}, + }); + check_results_by_node( + graph_cache, "ns1", "node1", + { + {"topic2", {"Str"}}, + }, + { + {"topic3", {"Bool"}}, + }); + check_results_by_node(graph_cache, "ns1", "node2"); + check_results_by_node(graph_cache, "ns1", "node3"); + check_results_by_node( + graph_cache, "ns2", "node1", + { + {"topic2", {"Str"}}, + }); + check_results_by_node(graph_cache, "ns3", "node1"); + check_results_by_node(graph_cache, "ns", "some_random_node"); + check_results_by_topic(graph_cache, "topic1", 2, 2); + check_results_by_topic(graph_cache, "topic2", 3, 0); + check_results_by_topic(graph_cache, "topic3", 0, 1); + check_results_by_topic(graph_cache, "some_topic", 0, 0); + + graph_cache.clear_on_change_callback(); + change_callback_called = false; + + // Add some readers and writers. + add_entities( + graph_cache, + { + // topic1 + {"reader6", "remote_participant", "topic1", "Str", true}, + {"reader7", "remote_participant", "topic1", "Custom", true}, + // topic2 + {"writer4", "remote_participant", "topic2", "Str", false}, + // topic4 + {"writer5", "remote_participant", "topic4", "Custom", false}, + {"writer6", "remote_participant", "topic4", "Bool", false}, + }); + + // Associate them with a remote participant. + auto msg = get_participant_entities_info_msg( + { + "remote_participant", + { + { + "ns3", + "node2", + {"reader6"}, + {"writer4", "writer5"} + }, + { + "ns4", + "node1", + {"reader7"}, + {} + } + } + }); + graph_cache.update_participant_entities(msg); + EXPECT_FALSE(change_callback_called); + + // Check state. + check_results( + graph_cache, + { + {"ns1", "node1"}, + {"ns1", "node2"}, + {"ns1", "node3"}, + {"ns2", "node1"}, + {"ns3", "node1"}, + {"ns3", "node2"}, + {"ns4", "node1"}, + }, + { + {"topic1", {"Custom", "Float", "Int", "Str"}}, + {"topic2", {"Str"}}, + {"topic3", {"Bool"}}, + {"topic4", {"Bool", "Custom"}}, + }); + check_results_by_node( + graph_cache, "ns1", "node1", + { + {"topic2", {"Str"}}, + }, + { + {"topic3", {"Bool"}}, + }); + check_results_by_node(graph_cache, "ns1", "node2"); + check_results_by_node(graph_cache, "ns1", "node3"); + check_results_by_node( + graph_cache, "ns2", "node1", + { + {"topic2", {"Str"}}, + }); + check_results_by_node(graph_cache, "ns3", "node1"); + check_results_by_node( + graph_cache, "ns3", "node2", + { + {"topic1", {"Str"}}, + }, + { + {"topic2", {"Str"}}, + {"topic4", {"Custom"}}, + }); + check_results_by_node( + graph_cache, "ns4", "node1", + { + {"topic1", {"Custom"}}, + }); + check_results_by_node(graph_cache, "ns", "some_random_node"); + check_results_by_topic(graph_cache, "topic1", 4, 2); + check_results_by_topic(graph_cache, "topic2", 3, 1); + check_results_by_topic(graph_cache, "topic3", 0, 1); + check_results_by_topic( + graph_cache, "topic4", {}, { + {"writer5", "ns3", "node2", "Custom"}, + {"writer6", "_NODE_NAMESPACE_UNKNOWN_", "_NODE_NAME_UNKNOWN_", "Bool"}, + }); + check_results_by_topic(graph_cache, "some_topic", 0, 0); + + // Remove some readers and writers. + remove_entities( + graph_cache, + { + // topic1 + {"reader6", "remote_participant", "topic1", "Str", true}, + // topic2 + {"writer4", "remote_participant", "topic2", "Str", false}, + // topic4 + {"writer5", "remote_participant", "topic4", "Custom", false}, + {"writer6", "remote_participant", "topic4", "Bool", false}, + }); + + // Check state. + check_results( + graph_cache, + { + {"ns1", "node1"}, + {"ns1", "node2"}, + {"ns1", "node3"}, + {"ns2", "node1"}, + {"ns3", "node1"}, + {"ns3", "node2"}, + {"ns4", "node1"}, + }, + { + {"topic1", {"Custom", "Float", "Int", "Str"}}, + {"topic2", {"Str"}}, + {"topic3", {"Bool"}}, + }); + check_results_by_node( + graph_cache, "ns1", "node1", + { + {"topic2", {"Str"}}, + }, + { + {"topic3", {"Bool"}}, + }); + check_results_by_node(graph_cache, "ns1", "node2"); + check_results_by_node(graph_cache, "ns1", "node3"); + check_results_by_node( + graph_cache, "ns2", "node1", + { + {"topic2", {"Str"}}, + }); + check_results_by_node(graph_cache, "ns3", "node1"); + check_results_by_node(graph_cache, "ns3", "node2"); + check_results_by_node( + graph_cache, "ns4", "node1", + { + {"topic1", {"Custom"}}, + }); + check_results_by_node(graph_cache, "ns", "some_random_node"); + check_results_by_topic(graph_cache, "topic1", 3, 2); + check_results_by_topic(graph_cache, "topic2", 3, 0); + check_results_by_topic(graph_cache, "topic3", 0, 1); + check_results_by_topic(graph_cache, "topic4", 0, 0); + check_results_by_topic(graph_cache, "some_topic", 0, 0); + + // Associate them with a remote participant. + msg = get_participant_entities_info_msg( + { + "remote_participant", + { + { + "ns4", + "node1", + {"reader7"}, + {} + } + } + }); + graph_cache.update_participant_entities(msg); + + // Check state. + check_results( + graph_cache, + { + {"ns1", "node1"}, + {"ns1", "node2"}, + {"ns1", "node3"}, + {"ns2", "node1"}, + {"ns3", "node1"}, + {"ns4", "node1"}, + }, + { + {"topic1", {"Custom", "Float", "Int", "Str"}}, + {"topic2", {"Str"}}, + {"topic3", {"Bool"}}, + }); + check_results_by_node( + graph_cache, "ns1", "node1", + { + {"topic2", {"Str"}}, + }, + { + {"topic3", {"Bool"}}, + }); + check_results_by_node(graph_cache, "ns1", "node2"); + check_results_by_node(graph_cache, "ns1", "node3"); + check_results_by_node( + graph_cache, "ns2", "node1", + { + {"topic2", {"Str"}}, + }); + check_results_by_node(graph_cache, "ns3", "node1"); + check_results_by_node(graph_cache, "ns3", "node2"); + check_results_by_node( + graph_cache, "ns4", "node1", + { + {"topic1", {"Custom"}}, + }); + check_results_by_node(graph_cache, "ns", "some_random_node"); + check_results_by_topic(graph_cache, "topic1", 3, 2); + check_results_by_topic(graph_cache, "topic2", 3, 0); + check_results_by_topic(graph_cache, "topic3", 0, 1); + check_results_by_topic(graph_cache, "topic4", 0, 0); + check_results_by_topic(graph_cache, "some_topic", 0, 0); + + // Remove remote participant + msg = get_participant_entities_info_msg( + { + "remote_participant", + {} + }); + graph_cache.update_participant_entities(msg); + + remove_participants(graph_cache, {"remote_participant"}); + + // Remove remaining entities + remove_entities( + graph_cache, + { + // topic1 + {"reader7", "remote_participant", "topic1", "Custom", true}, + }); + + // Check state. + check_results( + graph_cache, + { + {"ns1", "node1"}, + {"ns1", "node2"}, + {"ns1", "node3"}, + {"ns2", "node1"}, + {"ns3", "node1"}, + }, + { + {"topic1", {"Float", "Int", "Str"}}, + {"topic2", {"Str"}}, + {"topic3", {"Bool"}}, + }); + check_results_by_node( + graph_cache, "ns1", "node1", + { + {"topic2", {"Str"}}, + }, + { + {"topic3", {"Bool"}}, + }); + check_results_by_node(graph_cache, "ns1", "node2"); + check_results_by_node(graph_cache, "ns1", "node3"); + check_results_by_node( + graph_cache, "ns2", "node1", + { + {"topic2", {"Str"}}, + }); + check_results_by_node(graph_cache, "ns3", "node1"); + check_results_by_node(graph_cache, "ns", "some_random_node"); + check_results_by_topic(graph_cache, "topic1", 2, 2); + check_results_by_topic(graph_cache, "topic2", 3, 0); + check_results_by_topic(graph_cache, "topic3", 0, 1); + check_results_by_topic(graph_cache, "topic4", 0, 0); + check_results_by_topic(graph_cache, "some_topic", 0, 0); + + // Remove some local nodes + remove_nodes( + graph_cache, + { + {"participant1", "ns1", "node2"}, + {"participant1", "ns2", "node1"}, + {"participant2", "ns1", "node3"}, + {"participant2", "ns3", "node1"}, + }); + + // Remove some local participants + remove_participants( + graph_cache, + {"participant2", "participant3"}); + + // Remove some entities + remove_entities( + graph_cache, + { + // topic1 + {"reader1", "participant1", "topic1", "Str", true}, + {"reader2", "participant1", "topic1", "Float", true}, + {"writer1", "participant2", "topic1", "Int", false}, + {"writer2", "participant2", "topic1", "Str", false}, + // topic2 + {"reader3", "participant1", "topic2", "Str", true}, + {"reader4", "participant1", "topic2", "Str", true}, + {"reader5", "participant2", "topic2", "Str", true}, + }); + + // Check state. + check_results( + graph_cache, + { + {"ns1", "node1"}, + }, + { + {"topic3", {"Bool"}}, + }); + check_results_by_node( + graph_cache, "ns1", "node1", + {}, + { + {"topic3", {"Bool"}}, + }); + check_results_by_node(graph_cache, "ns1", "node2"); + check_results_by_node(graph_cache, "ns1", "node3"); + check_results_by_node(graph_cache, "ns2", "node1"); + check_results_by_node(graph_cache, "ns3", "node1"); + check_results_by_node(graph_cache, "ns", "some_random_node"); + check_results_by_topic(graph_cache, "topic1", 0, 0); + check_results_by_topic(graph_cache, "topic2", 0, 0); + check_results_by_topic(graph_cache, "topic3", 0, 1); + check_results_by_topic(graph_cache, "topic4", 0, 0); + check_results_by_topic(graph_cache, "some_topic", 0, 0); + + // Remove some local nodes + remove_nodes( + graph_cache, + { + {"participant1", "ns1", "node1"}, + }); + + // Remove every remaining participant, node, entity + remove_participants( + graph_cache, + {"participant1"}); + + // Remove some entities + remove_entities( + graph_cache, + { + // topic3 + {"writer3", "participant1", "topic3", "Bool", false}, + }); + + // Check state. + check_results(graph_cache); + check_results_by_node(graph_cache, "ns1", "node1"); + check_results_by_node(graph_cache, "ns1", "node2"); + check_results_by_node(graph_cache, "ns1", "node3"); + check_results_by_node(graph_cache, "ns2", "node1"); + check_results_by_node(graph_cache, "ns3", "node1"); + check_results_by_node(graph_cache, "ns", "some_random_node"); + check_results_by_topic(graph_cache, "topic1", 0, 0); + check_results_by_topic(graph_cache, "topic2", 0, 0); + check_results_by_topic(graph_cache, "topic3", 0, 0); + check_results_by_topic(graph_cache, "topic4", 0, 0); + check_results_by_topic(graph_cache, "some_topic", 0, 0); + + EXPECT_FALSE(change_callback_called); +} + +TEST(test_graph_cache, test_operator) +{ + GraphCache graph_cache; + + // Add one participant. + add_participants(graph_cache, {"participant1"}); + + std::string graph_cache_case1_str = "---------------------------------\n" + "Graph cache:\n" + " Discovered data writers:\n" + " Discovered data readers:\n" + " Discovered participants:\n" + " gid: '70.61.72.74.69.63.69.70.61.6e.74.31.0.0.0.0.0.0.0.0.0.0.0.0\n" + " enclave name '\n" + " nodes:\n" + "---------------------------------\n"; + + std::ostringstream stream_case1; + stream_case1 << graph_cache; + ASSERT_STREQ(stream_case1.str().c_str(), graph_cache_case1_str.c_str()); + + std::string graph_cache_case2_str = "---------------------------------\n" + "Graph cache:\n" + " Discovered data writers:\n" + " Discovered data readers:\n" + " Discovered participants:\n" + " gid: '70.61.72.74.69.63.69.70.61.6e.74.31.0.0.0.0.0.0.0.0.0.0.0.0\n" + " enclave name '\n" + " nodes:\n" + " namespace: 'ns1' name: 'node1'\n" + " associated data readers gids:\n" + " associated data writers gids:\n" + " namespace: 'ns1' name: 'node2'\n" + " associated data readers gids:\n" + " associated data writers gids:\n" + " namespace: 'ns2' name: 'node1'\n" + " associated data readers gids:\n" + " associated data writers gids:\n" + "---------------------------------\n"; + + // Add some nodes. + check_participant_entities_msg( + add_nodes( + graph_cache, { + {"participant1", "ns1", "node1"}, + {"participant1", "ns1", "node2"}, + {"participant1", "ns2", "node1"}}), + { + "participant1", { + {"ns1", "node1", {}, {}}, + {"ns1", "node2", {}, {}}, + {"ns2", "node1", {}, {}}, + } + }); + + std::ostringstream stream_case2; + stream_case2 << graph_cache; + ASSERT_STREQ(stream_case2.str().c_str(), graph_cache_case2_str.c_str()); + + std::string graph_cache_case3_str = "---------------------------------\n" + "Graph cache:\n" + " Discovered data writers:\n" + " gid: '77.72.69.74.65.72.31.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0'," + " topic name: 'topic1', topic_type: 'Str'\n" + " Discovered data readers:\n" + " gid: '72.65.61.64.65.72.31.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0'," + " topic name: 'topic1', topic_type: 'Str'\n" + " Discovered participants:\n" + " gid: '70.61.72.74.69.63.69.70.61.6e.74.31.0.0.0.0.0.0.0.0.0.0.0.0\n" + " enclave name '\n" + " nodes:\n" + " namespace: 'ns1' name: 'node1'\n" + " associated data readers gids:\n" + " 72.65.61.64.65.72.31.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0\n" + " associated data writers gids:\n" + " namespace: 'ns1' name: 'node2'\n" + " associated data readers gids:\n" + " associated data writers gids:\n" + " 77.72.69.74.65.72.31.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0.0\n" + " namespace: 'ns2' name: 'node1'\n" + " associated data readers gids:\n" + " associated data writers gids:\n" + "---------------------------------\n"; + + add_entities( + graph_cache, + { + // topic1 + {"reader1", "participant1", "topic1", "Str", true}, + {"writer1", "participant1", "topic1", "Str", false}, + }); + + // Associate entities + associate_entities( + graph_cache, + { + // participant1, ns1, node1 + {"reader1", true, "participant1", "ns1", "node1"}, + {"writer1", false, "participant1", "ns1", "node2"}, + }); + std::ostringstream stream_case3; + stream_case3 << graph_cache; + ASSERT_STREQ(stream_case3.str().c_str(), graph_cache_case3_str.c_str()); +} + +TEST(test_graph_cache, bad_arguments) +{ + GraphCache graph_cache; + + EXPECT_EQ( + graph_cache.get_reader_count("topic_name", nullptr), + RMW_RET_INVALID_ARGUMENT); + + EXPECT_EQ( + graph_cache.get_writer_count("topic_name", nullptr), + RMW_RET_INVALID_ARGUMENT); + + { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcutils_string_array_t names = rcutils_get_zero_initialized_string_array(); + rmw_ret_t ret = rcutils_string_array_init(&names, 3, &allocator); + EXPECT_EQ(ret, RMW_RET_OK) << rcutils_get_error_string().str; + rcutils_reset_error(); + rcutils_string_array_t namespaces = rcutils_get_zero_initialized_string_array(); + ret = graph_cache.get_node_names(&names, &namespaces, nullptr, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << rcutils_get_error_string().str; + rcutils_reset_error(); + ret = rcutils_string_array_fini(&names); + EXPECT_EQ(ret, RMW_RET_OK) << rcutils_get_error_string().str; + rcutils_reset_error(); + } + + { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcutils_string_array_t names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t namespaces = rcutils_get_zero_initialized_string_array(); + rmw_ret_t ret = rcutils_string_array_init(&namespaces, 3, &allocator); + EXPECT_EQ(ret, RMW_RET_OK) << rcutils_get_error_string().str; + rcutils_reset_error(); + ret = graph_cache.get_node_names(&names, &namespaces, nullptr, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << rcutils_get_error_string().str; + rcutils_reset_error(); + ret = rcutils_string_array_fini(&namespaces); + EXPECT_EQ(ret, RMW_RET_OK) << rcutils_get_error_string().str; + rcutils_reset_error(); + } + + { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcutils_string_array_t names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t namespaces = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t enclaves = rcutils_get_zero_initialized_string_array(); + rmw_ret_t ret = rcutils_string_array_init(&enclaves, 3, &allocator); + EXPECT_EQ(ret, RMW_RET_OK) << rcutils_get_error_string().str; + rcutils_reset_error(); + ret = graph_cache.get_node_names(&names, &namespaces, &enclaves, &allocator); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT) << rcutils_get_error_string().str; + rcutils_reset_error(); + ret = rcutils_string_array_fini(&enclaves); + EXPECT_EQ(ret, RMW_RET_OK) << rcutils_get_error_string().str; + rcutils_reset_error(); + } + + { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rcutils_allocator_t zero_allocator = rcutils_get_zero_initialized_allocator(); + rmw_names_and_types_t names_and_types = rmw_get_zero_initialized_names_and_types(); + rmw_ret_t ret = graph_cache.get_writer_names_and_types_by_node( + "node_name", + "node_namespace", + identity_demangle, + identity_demangle, + &allocator, + &names_and_types); + EXPECT_EQ(ret, RMW_RET_NODE_NAME_NON_EXISTENT); + rcutils_reset_error(); + ret = graph_cache.get_writer_names_and_types_by_node( + "node_name", + "node_namespace", + identity_demangle, + identity_demangle, + &zero_allocator, + &names_and_types); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT); + rcutils_reset_error(); + ret = graph_cache.get_writer_names_and_types_by_node( + "node_name", + "node_namespace", + identity_demangle, + identity_demangle, + &allocator, + nullptr); + EXPECT_EQ(ret, RMW_RET_INVALID_ARGUMENT); + rcutils_reset_error(); + } + + // Add one participant. + add_participants(graph_cache, {"participant1"}); + { + rmw_topic_endpoint_info_array_t topic_endpoint_info_array_sub = + rmw_get_zero_initialized_topic_endpoint_info_array(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_ret_t ret = graph_cache.get_writers_info_by_topic( + "topic1", + identity_demangle, + &allocator, + &topic_endpoint_info_array_sub); + EXPECT_EQ(ret, RMW_RET_OK); + ASSERT_EQ(topic_endpoint_info_array_sub.size, 0u) << "Expected one publisher"; + EXPECT_EQ(topic_endpoint_info_array_sub.info_array, nullptr); + } + + // Add some nodes. + add_nodes( + graph_cache, { + {"participant1", "ns1", "node1"}, + {"participant1", "ns1", "node2"}, + {"participant1", "ns2", "node1"}}); + + add_entities( + graph_cache, + { + // topic1 + {"reader1", "some_participant", "topic1", "Str", true}, + {"writer1", "some_participant", "topic1", "Str", false}, + }); + + // Associate entities + associate_entities( + graph_cache, + { + // participant1, ns1, node1 + {"reader1", true, "participant1", "ns1", "node1"}, + {"writer1", false, "participant1", "ns1", "node2"}, + }); + + { + rcutils_allocator_t failing_allocator = get_failing_allocator(); + rcutils_string_array_t names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t namespaces = rcutils_get_zero_initialized_string_array(); + rmw_ret_t ret = graph_cache.get_node_names(&names, &namespaces, nullptr, &failing_allocator); + EXPECT_EQ(ret, RMW_RET_BAD_ALLOC); + rcutils_reset_error(); + } + + { + rmw_topic_endpoint_info_array_t topic_endpoint_info_array_sub = + rmw_get_zero_initialized_topic_endpoint_info_array(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_ret_t ret = graph_cache.get_writers_info_by_topic( + "topic1", + identity_demangle, + &allocator, + &topic_endpoint_info_array_sub); + EXPECT_EQ(ret, RMW_RET_OK); + ASSERT_EQ(topic_endpoint_info_array_sub.size, 1u) << "Expected one publisher"; + rmw_topic_endpoint_info_t topic_endpoint_info_pub = topic_endpoint_info_array_sub.info_array[0]; + EXPECT_STREQ(topic_endpoint_info_pub.node_name, "_CREATED_BY_BARE_DDS_APP_"); + EXPECT_STREQ(topic_endpoint_info_pub.node_namespace, "_CREATED_BY_BARE_DDS_APP_"); + EXPECT_STREQ(topic_endpoint_info_pub.topic_type, "Str"); + rcutils_reset_error(); + } + { + rmw_topic_endpoint_info_array_t topic_endpoint_info_array_sub = + rmw_get_zero_initialized_topic_endpoint_info_array(); + rcutils_allocator_t failing_allocator = get_failing_allocator(); + rmw_ret_t ret = graph_cache.get_writers_info_by_topic( + "topic1", + identity_demangle, + &failing_allocator, + &topic_endpoint_info_array_sub); + EXPECT_EQ(ret, RMW_RET_BAD_ALLOC); + ASSERT_EQ(topic_endpoint_info_array_sub.size, 0u) << "Expected one publisher"; + EXPECT_EQ(topic_endpoint_info_array_sub.info_array, nullptr); + rcutils_reset_error(); + } + { + rmw_topic_endpoint_info_array_t topic_endpoint_info_array_sub = + rmw_get_zero_initialized_topic_endpoint_info_array(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_ret_t ret = graph_cache.get_readers_info_by_topic( + "topic1", + identity_demangle, + &allocator, + &topic_endpoint_info_array_sub); + EXPECT_EQ(ret, RMW_RET_OK); + ASSERT_EQ(topic_endpoint_info_array_sub.size, 1u) << "Expected one publisher"; + rmw_topic_endpoint_info_t topic_endpoint_info_pub = topic_endpoint_info_array_sub.info_array[0]; + EXPECT_STREQ(topic_endpoint_info_pub.node_name, "_CREATED_BY_BARE_DDS_APP_"); + EXPECT_STREQ(topic_endpoint_info_pub.node_namespace, "_CREATED_BY_BARE_DDS_APP_"); + EXPECT_STREQ(topic_endpoint_info_pub.topic_type, "Str"); + rcutils_reset_error(); + } + { + rmw_topic_endpoint_info_array_t topic_endpoint_info_array_sub = + rmw_get_zero_initialized_topic_endpoint_info_array(); + rcutils_allocator_t failing_allocator = get_failing_allocator(); + rmw_ret_t ret = graph_cache.get_readers_info_by_topic( + "topic1", + identity_demangle, + &failing_allocator, + &topic_endpoint_info_array_sub); + EXPECT_EQ(ret, RMW_RET_BAD_ALLOC); + ASSERT_EQ(topic_endpoint_info_array_sub.size, 0u) << "Expected one publisher"; + EXPECT_EQ(topic_endpoint_info_array_sub.info_array, nullptr); + rcutils_reset_error(); + } +} + +class TestGraphCache : public ::testing::Test +{ +public: + void SetUp() + { + add_participants(graph_cache_, {"participant1"}); + add_entities( + graph_cache_, + { + // topic1 + {"reader1", "participant1", "topic1", "Str", true}, + {"writer1", "participant1", "topic1", "Str", false}, + }); + add_nodes( + graph_cache_, { + {"participant1", "ns1", "node1"}, + {"participant1", "ns1", "node2"}, + {"participant1", "ns2", "node1"}}); + } + +protected: + GraphCache graph_cache_; +}; + +TEST_F(TestGraphCache, get_writers_info_by_topic_maybe_fail) +{ + RCUTILS_FAULT_INJECTION_TEST( + { + rmw_topic_endpoint_info_array_t info = rmw_get_zero_initialized_topic_endpoint_info_array(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + rmw_ret_t ret = graph_cache_.get_writers_info_by_topic( + "topic1", + identity_demangle, + &allocator, + &info); + if (RMW_RET_OK == ret) { + ret = rmw_topic_endpoint_info_array_fini(&info, &allocator); + if (RMW_RET_OK != ret) { + // If fault injection causes fini to fail, it should work the second time. + EXPECT_EQ(RMW_RET_OK, rmw_topic_endpoint_info_array_fini(&info, &allocator)); + } + } else { + rcutils_reset_error(); + } + }); +} + +TEST_F(TestGraphCache, get_node_names_maybe_fail) +{ + RCUTILS_FAULT_INJECTION_TEST( + { + rcutils_string_array_t names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t namespaces = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t enclaves = rcutils_get_zero_initialized_string_array(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_ret_t ret = graph_cache_.get_node_names(&names, &namespaces, &enclaves, &allocator); + if (RMW_RET_OK == ret) { + // rcutils_string_array_fini is not under test, so disable fault injection test here. + int64_t fault_injection_count = rcutils_fault_injection_get_count(); + rcutils_fault_injection_set_count(RCUTILS_FAULT_INJECTION_NEVER_FAIL); + + EXPECT_EQ(RCUTILS_RET_OK, rcutils_string_array_fini(&names)); + EXPECT_EQ(RCUTILS_RET_OK, rcutils_string_array_fini(&namespaces)); + EXPECT_EQ(RCUTILS_RET_OK, rcutils_string_array_fini(&enclaves)); + + rcutils_fault_injection_set_count(fault_injection_count); + } else { + rcutils_reset_error(); + } + }); +} + +TEST_F(TestGraphCache, get_names_and_types_maybe_fail) +{ + RCUTILS_FAULT_INJECTION_TEST( + { + DemangleFunctionT demangle_topic = identity_demangle; + DemangleFunctionT demangle_type = identity_demangle; + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_names_and_types_t names_and_types = rmw_get_zero_initialized_names_and_types(); + rmw_ret_t ret = graph_cache_.get_names_and_types( + demangle_topic, + demangle_type, + &allocator, + &names_and_types); + if (RMW_RET_OK == ret) { + ret = rmw_names_and_types_fini(&names_and_types); + if (RMW_RET_OK != ret) { + // If fault injection causes fini to fail, it should work the second time. + EXPECT_EQ(RMW_RET_OK, rmw_names_and_types_fini(&names_and_types)); + } + } else { + rcutils_reset_error(); + } + }); +}