From 5486297dffa0f9d27a3d3f50487a4d52333d171f Mon Sep 17 00:00:00 2001 From: ivanpauno Date: Tue, 3 Sep 2019 13:20:53 -0700 Subject: [PATCH 01/37] Create one Participant per Context instead of one per Node Add example test of how to access message type supports from rmw_dds_common Signed-off-by: ivanpauno Move from one participant per node to one participant per context. Signed-off-by: ivanpauno Reimplemented rmw_node_info_and_types Signed-off-by: Ivan Santiago Paunovic Implement rmw_count Signed-off-by: Ivan Santiago Paunovic Please linter Signed-off-by: Ivan Santiago Paunovic Correct rmw_service_server_is_available Signed-off-by: Ivan Santiago Paunovic Implement rmw_service_names_and_types and rmw_topic_names_and_types Signed-off-by: Ivan Santiago Paunovic Add guard condition to listener thread Signed-off-by: Ivan Santiago Paunovic Stop using group data. Use the new group cache Signed-off-by: Ivan Santiago Paunovic Delete old group data stuff Signed-off-by: Ivan Santiago Paunovic Further changes after rebasing with master Signed-off-by: Ivan Santiago Paunovic Update after changes in rmw_dds_common Signed-off-by: Ivan Santiago Paunovic Delete old comment. Update default domain id macro Signed-off-by: Ivan Santiago Paunovic Address some reviewer comments Signed-off-by: Ivan Santiago Paunovic Fix some upstream test failures Signed-off-by: Ivan Santiago Paunovic Correct linter failure Signed-off-by: Ivan Santiago Paunovic Add node sanity check in __rmw_destroy_service Signed-off-by: Ivan Santiago Paunovic Correct error in __rmw_destroy_publisher Signed-off-by: Ivan Santiago Paunovic Addressed per review comments Signed-off-by: Ivan Santiago Paunovic Minimal cleanup Signed-off-by: Ivan Santiago Paunovic Updated rmw_fastrtps_dynamic_cpp Signed-off-by: Ivan Santiago Paunovic Address peer review comments Signed-off-by: Ivan Santiago Paunovic Create participant in first use, destruct it after last node is destroyed Signed-off-by: Ivan Santiago Paunovic Address PR comments Signed-off-by: Ivan Santiago Paunovic Add context name and namespace Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/CMakeLists.txt | 15 + .../include/rmw_fastrtps_cpp/publisher.hpp | 36 ++ .../rmw_fastrtps_cpp/register_node.hpp | 41 ++ .../include/rmw_fastrtps_cpp/subscription.hpp | 38 ++ rmw_fastrtps_cpp/package.xml | 2 + rmw_fastrtps_cpp/src/get_participant.cpp | 4 +- rmw_fastrtps_cpp/src/gid__type_support.cpp | 52 ++ rmw_fastrtps_cpp/src/listener_thread.cpp | 170 +++++++ rmw_fastrtps_cpp/src/listener_thread.hpp | 30 ++ .../src/node_entities_info__type_support.cpp | 52 ++ ...articipant_entities_info__type_support.cpp | 52 ++ rmw_fastrtps_cpp/src/publisher.cpp | 182 +++++++ rmw_fastrtps_cpp/src/register_node.cpp | 217 ++++++++ rmw_fastrtps_cpp/src/rmw_client.cpp | 56 ++- rmw_fastrtps_cpp/src/rmw_init.cpp | 96 +++- rmw_fastrtps_cpp/src/rmw_node.cpp | 23 +- rmw_fastrtps_cpp/src/rmw_publisher.cpp | 189 ++----- rmw_fastrtps_cpp/src/rmw_security.cpp | 24 + rmw_fastrtps_cpp/src/rmw_service.cpp | 47 +- rmw_fastrtps_cpp/src/rmw_subscription.cpp | 171 ++----- rmw_fastrtps_cpp/src/subscription.cpp | 173 +++++++ rmw_fastrtps_cpp/test/test_gid_message.cpp | 42 ++ rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 14 +- .../rmw_fastrtps_dynamic_cpp/publisher.hpp | 36 ++ .../register_node.hpp | 41 ++ .../rmw_fastrtps_dynamic_cpp/subscription.hpp | 38 ++ rmw_fastrtps_dynamic_cpp/package.xml | 2 + .../src/get_participant.cpp | 3 +- .../src/gid__type_support.cpp | 52 ++ .../src/listener_thread.cpp | 170 +++++++ .../src/listener_thread.hpp | 30 ++ .../src/node_entities_info__type_support.cpp | 52 ++ ...articipant_entities_info__type_support.cpp | 52 ++ rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 216 ++++++++ .../src/register_node.cpp | 217 ++++++++ rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 56 ++- rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp | 54 +- rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp | 19 +- .../src/rmw_publisher.cpp | 201 ++------ rmw_fastrtps_dynamic_cpp/src/rmw_security.cpp | 24 + rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 44 +- .../src/rmw_subscription.cpp | 185 ++----- rmw_fastrtps_dynamic_cpp/src/subscription.cpp | 194 +++++++ rmw_fastrtps_shared_cpp/CMakeLists.txt | 6 + .../create_rmw_gid.hpp | 33 ++ .../custom_participant_info.hpp | 131 ++--- .../custom_subscriber_info.hpp | 5 +- .../include/rmw_fastrtps_shared_cpp/names.hpp | 33 +- .../namespace_prefix.hpp | 10 + .../rmw_fastrtps_shared_cpp/participant.hpp | 39 ++ .../rmw_fastrtps_shared_cpp/publisher.hpp | 32 ++ .../include/rmw_fastrtps_shared_cpp/qos.hpp | 11 +- .../rmw_fastrtps_shared_cpp/rmw_common.hpp | 10 +- .../rmw_context_impl.hpp | 28 ++ .../rmw_fastrtps_shared_cpp/subscription.hpp | 32 ++ rmw_fastrtps_shared_cpp/package.xml | 2 + .../src/create_rmw_gid.cpp | 36 ++ rmw_fastrtps_shared_cpp/src/demangle.cpp | 95 ++-- rmw_fastrtps_shared_cpp/src/demangle.hpp | 23 +- .../src/namespace_prefix.cpp | 10 + rmw_fastrtps_shared_cpp/src/participant.cpp | 242 +++++++++ rmw_fastrtps_shared_cpp/src/publisher.cpp | 69 +++ rmw_fastrtps_shared_cpp/src/qos.cpp | 1 + rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 27 + rmw_fastrtps_shared_cpp/src/rmw_count.cpp | 98 +--- .../src/rmw_get_topic_endpoint_info.cpp | 261 ++-------- rmw_fastrtps_shared_cpp/src/rmw_node.cpp | 347 ++----------- .../src/rmw_node_info_and_types.cpp | 473 ++++-------------- .../src/rmw_node_names.cpp | 66 +-- rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp | 64 +-- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 34 +- .../src/rmw_service_names_and_types.cpp | 109 +--- .../src/rmw_service_server_is_available.cpp | 28 +- .../src/rmw_subscription.cpp | 66 +-- .../src/rmw_topic_names_and_types.cpp | 126 +---- rmw_fastrtps_shared_cpp/src/subscription.cpp | 78 +++ rmw_fastrtps_shared_cpp/test/CMakeLists.txt | 6 - .../test/test_topic_cache.cpp | 262 ---------- 78 files changed, 3894 insertions(+), 2411 deletions(-) create mode 100644 rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp create mode 100644 rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/register_node.hpp create mode 100644 rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp create mode 100644 rmw_fastrtps_cpp/src/gid__type_support.cpp create mode 100644 rmw_fastrtps_cpp/src/listener_thread.cpp create mode 100644 rmw_fastrtps_cpp/src/listener_thread.hpp create mode 100644 rmw_fastrtps_cpp/src/node_entities_info__type_support.cpp create mode 100644 rmw_fastrtps_cpp/src/participant_entities_info__type_support.cpp create mode 100644 rmw_fastrtps_cpp/src/publisher.cpp create mode 100644 rmw_fastrtps_cpp/src/register_node.cpp create mode 100644 rmw_fastrtps_cpp/src/rmw_security.cpp create mode 100644 rmw_fastrtps_cpp/src/subscription.cpp create mode 100644 rmw_fastrtps_cpp/test/test_gid_message.cpp create mode 100644 rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp create mode 100644 rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/register_node.hpp create mode 100644 rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp create mode 100644 rmw_fastrtps_dynamic_cpp/src/gid__type_support.cpp create mode 100644 rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp create mode 100644 rmw_fastrtps_dynamic_cpp/src/listener_thread.hpp create mode 100644 rmw_fastrtps_dynamic_cpp/src/node_entities_info__type_support.cpp create mode 100644 rmw_fastrtps_dynamic_cpp/src/participant_entities_info__type_support.cpp create mode 100644 rmw_fastrtps_dynamic_cpp/src/publisher.cpp create mode 100644 rmw_fastrtps_dynamic_cpp/src/register_node.cpp create mode 100644 rmw_fastrtps_dynamic_cpp/src/rmw_security.cpp create mode 100644 rmw_fastrtps_dynamic_cpp/src/subscription.cpp create mode 100644 rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/create_rmw_gid.hpp create mode 100644 rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp create mode 100644 rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/publisher.hpp create mode 100644 rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_context_impl.hpp create mode 100644 rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/subscription.hpp create mode 100644 rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp create mode 100644 rmw_fastrtps_shared_cpp/src/participant.cpp create mode 100644 rmw_fastrtps_shared_cpp/src/publisher.cpp create mode 100644 rmw_fastrtps_shared_cpp/src/subscription.cpp delete mode 100644 rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 497953dcf..41ff3b908 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -32,6 +32,7 @@ endif() find_package(ament_cmake_ros REQUIRED) find_package(rcutils REQUIRED) +find_package(rmw_dds_common REQUIRED) find_package(rmw_fastrtps_shared_cpp REQUIRED) find_package(fastrtps_cmake_module REQUIRED) @@ -53,6 +54,9 @@ add_library(rmw_fastrtps_cpp src/get_service.cpp src/get_subscriber.cpp src/identifier.cpp + src/listener_thread.cpp + src/publisher.cpp + src/register_node.cpp src/rmw_logging.cpp src/rmw_client.cpp src/rmw_compare_gids_equal.cpp @@ -71,6 +75,7 @@ add_library(rmw_fastrtps_cpp src/rmw_publisher.cpp src/rmw_request.cpp src/rmw_response.cpp + src/rmw_security.cpp src/rmw_serialize.cpp src/rmw_service.cpp src/rmw_service_names_and_types.cpp @@ -82,7 +87,11 @@ add_library(rmw_fastrtps_cpp src/rmw_wait.cpp src/rmw_wait_set.cpp src/serialization_format.cpp + src/subscription.cpp src/type_support_common.cpp + src/gid__type_support.cpp + src/node_entities_info__type_support.cpp + src/participant_entities_info__type_support.cpp ) target_link_libraries(rmw_fastrtps_cpp fastcdr fastrtps) @@ -92,6 +101,7 @@ ament_target_dependencies(rmw_fastrtps_cpp "rcutils" "rosidl_typesupport_fastrtps_c" "rosidl_typesupport_fastrtps_cpp" + "rmw_dds_common" "rmw_fastrtps_shared_cpp" "rmw" "rosidl_generator_c" @@ -122,6 +132,11 @@ register_rmw_implementation( if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_gid_message test/test_gid_message.cpp) + if(TARGET test_gid_message) + target_link_libraries(test_gid_message ${PROJECT_NAME}) + endif() endif() ament_package( diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp new file mode 100644 index 000000000..e59b3ccd1 --- /dev/null +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/publisher.hpp @@ -0,0 +1,36 @@ +// Copyright 2017-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_FASTRTPS_CPP__PUBLISHER_HPP_ +#define RMW_FASTRTPS_CPP__PUBLISHER_HPP_ + +#include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" + +namespace rmw_fastrtps_cpp +{ + +rmw_publisher_t * +create_publisher( + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_publisher_options_t * publisher_options, + bool keyed, + bool create_publisher_listener); + +} // namespace rmw_fastrtps_cpp + +#endif // RMW_FASTRTPS_CPP__PUBLISHER_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/register_node.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/register_node.hpp new file mode 100644 index 000000000..2ab12b6b0 --- /dev/null +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/register_node.hpp @@ -0,0 +1,41 @@ +// 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_FASTRTPS_CPP__REGISTER_NODE_HPP_ +#define RMW_FASTRTPS_CPP__REGISTER_NODE_HPP_ + +#include "rmw/init.h" +#include "rmw/types.h" + +namespace rmw_fastrtps_cpp +{ + +/// Register node in context. +/** + * Function that should be called when creating a node, + * before using `context->impl`. + */ +rmw_ret_t +register_node(rmw_context_t * context); + +/// Unregister node in context. +/** + * Function that should be called when destroying a node. + */ +rmw_ret_t +unregister_node(rmw_context_t * context); + +} // namespace rmw_fastrtps_cpp + +#endif // RMW_FASTRTPS_CPP__REGISTER_NODE_HPP_ diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp new file mode 100644 index 000000000..a35d03f15 --- /dev/null +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/subscription.hpp @@ -0,0 +1,38 @@ +// 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_FASTRTPS_CPP__SUBSCRIPTION_HPP_ +#define RMW_FASTRTPS_CPP__SUBSCRIPTION_HPP_ + +#include "rmw/rmw.h" +#include "rmw/subscription_options.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" + +namespace rmw_fastrtps_cpp +{ + +rmw_subscription_t * +create_subscription( + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_subscription_options_t * subscription_options, + bool keyed, + bool create_subscription_listener); + +} // namespace rmw_fastrtps_cpp + +#endif // RMW_FASTRTPS_CPP__SUBSCRIPTION_HPP_ diff --git a/rmw_fastrtps_cpp/package.xml b/rmw_fastrtps_cpp/package.xml index 94d7105a5..1bc9da807 100644 --- a/rmw_fastrtps_cpp/package.xml +++ b/rmw_fastrtps_cpp/package.xml @@ -20,6 +20,7 @@ fastrtps_cmake_module rcutils rmw + rmw_dds_common rmw_fastrtps_shared_cpp rosidl_generator_c rosidl_generator_cpp @@ -30,6 +31,7 @@ fastrtps fastrtps_cmake_module rcutils + rmw_dds_common rmw_fastrtps_shared_cpp rmw rosidl_generator_c diff --git a/rmw_fastrtps_cpp/src/get_participant.cpp b/rmw_fastrtps_cpp/src/get_participant.cpp index c489f3d4c..120086460 100644 --- a/rmw_fastrtps_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_cpp/src/get_participant.cpp @@ -15,6 +15,8 @@ #include "rmw_fastrtps_cpp/get_participant.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + #include "rmw_fastrtps_cpp/identifier.hpp" namespace rmw_fastrtps_cpp @@ -29,7 +31,7 @@ get_participant(rmw_node_t * node) if (node->implementation_identifier != eprosima_fastrtps_identifier) { return nullptr; } - auto impl = static_cast(node->data); + auto impl = static_cast(node->context->impl->participant_info); return impl->participant; } diff --git a/rmw_fastrtps_cpp/src/gid__type_support.cpp b/rmw_fastrtps_cpp/src/gid__type_support.cpp new file mode 100644 index 000000000..1e6d41ee2 --- /dev/null +++ b/rmw_fastrtps_cpp/src/gid__type_support.cpp @@ -0,0 +1,52 @@ +// 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 "rosidl_generator_c/message_type_support_struct.h" +// rosidl_typesupport_cpp/message_type_support.hpp is installed by rosidl_generator_cpp +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rmw_dds_common/msg/gid__struct.hpp" +#include "rmw_dds_common/msg/gid__rosidl_typesupport_fastrtps_cpp.hpp" +#include "rmw_fastrtps_cpp/visibility_control.h" + +namespace rosidl_typesupport_cpp +{ + +template<> +RMW_FASTRTPS_CPP_PUBLIC +const rosidl_message_type_support_t * +get_message_type_support_handle() +{ + return ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_fastrtps_cpp, rmw_dds_common, msg, Gid)(); +} + +#ifdef __cplusplus +extern "C" +{ +#endif + +RMW_FASTRTPS_CPP_PUBLIC +const rosidl_message_type_support_t * +ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_cpp, rmw_dds_common, msg, Gid)() +{ + return get_message_type_support_handle(); +} + +#ifdef __cplusplus +} +#endif +} // namespace rosidl_typesupport_cpp diff --git a/rmw_fastrtps_cpp/src/listener_thread.cpp b/rmw_fastrtps_cpp/src/listener_thread.cpp new file mode 100644 index 000000000..590ed07ed --- /dev/null +++ b/rmw_fastrtps_cpp/src/listener_thread.cpp @@ -0,0 +1,170 @@ +// 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 "rcutils/macros.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/init.h" +#include "rmw/ret_types.h" +#include "rmw/rmw.h" +#include "rmw/types.h" +#include "rmw/impl/cpp/macros.hpp" + +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/gid_utils.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" + +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +#include "listener_thread.hpp" + +using rmw_dds_common::operator<<; + +static +void +node_listener(rmw_context_t * context); + +rmw_ret_t +rmw_fastrtps_cpp::run_listener_thread(rmw_context_t * context) +{ + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.store(true); + common_context->listener_thread_gc = rmw_create_guard_condition(context); + if (common_context->listener_thread_gc) { + try { + common_context->listener_thread = std::thread(node_listener, context); + return RMW_RET_OK; + } catch (const std::exception & exc) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to create std::thread: %s", exc.what()); + } catch (...) { + RMW_SET_ERROR_MSG("Failed to create std::thread"); + } + } else { + RMW_SET_ERROR_MSG("Failed to create guard condition"); + } + common_context->thread_is_running.store(false); + if (common_context->listener_thread_gc) { + if (RMW_RET_OK != rmw_destroy_guard_condition(common_context->listener_thread_gc)) { + fprintf( + stderr, + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" + RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); + } + } + return RMW_RET_ERROR; +} + +rmw_ret_t +rmw_fastrtps_cpp::join_listener_thread(rmw_context_t * context) +{ + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.exchange(false); + rmw_ret_t rmw_ret = rmw_trigger_guard_condition(common_context->listener_thread_gc); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + try { + common_context->listener_thread.join(); + } catch (const std::exception & exc) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to join std::thread: %s", exc.what()); + return RMW_RET_ERROR; + } catch (...) { + RMW_SET_ERROR_MSG("Failed to join std::thread"); + return RMW_RET_ERROR; + } + rmw_ret = rmw_destroy_guard_condition(common_context->listener_thread_gc); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + return RMW_RET_OK; +} + +#define TERMINATE(msg) \ + do { \ + fprintf( \ + stderr, \ + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ + RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) ": %s, terminating ...", \ + rmw_get_error_string().str); \ + std::terminate(); \ + } while (0) + +void +node_listener(rmw_context_t * context) +{ + assert(nullptr != context); + assert(nullptr != context->impl); + assert(nullptr != context->impl->common); + auto common_context = static_cast(context->impl->common); + while (common_context->thread_is_running.load()) { + assert(nullptr != common_context->sub); + assert(nullptr != common_context->sub->data); + void * subscriptions_buffer[] = {common_context->sub->data}; + void * guard_conditions_buffer[] = {common_context->listener_thread_gc->data}; + rmw_subscriptions_t subscriptions; + rmw_guard_conditions_t guard_conditions; + subscriptions.subscriber_count = 1; + subscriptions.subscribers = subscriptions_buffer; + guard_conditions.guard_condition_count = 1; + guard_conditions.guard_conditions = guard_conditions_buffer; + // number of conditions of a subscription is 2 + rmw_wait_set_t * wait_set = rmw_create_wait_set(context, 2); + if (nullptr == wait_set) { + TERMINATE("failed to create wait set"); + } + if (RMW_RET_OK != rmw_wait( + &subscriptions, + &guard_conditions, + nullptr, + nullptr, + nullptr, + wait_set, + nullptr)) + { + TERMINATE("rmw_wait failed"); + } + if (subscriptions_buffer[0]) { + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + bool taken; + if (RMW_RET_OK != rmw_take( + common_context->sub, + static_cast(&msg), + &taken, + nullptr)) + { + TERMINATE("rmw_take failed"); + } + if (taken) { + if (std::memcmp( + reinterpret_cast(common_context->gid.data), + reinterpret_cast(&msg.gid.data), + RMW_GID_STORAGE_SIZE) == 0) + { + // ignore local messages + continue; + } + common_context->graph_cache.update_participant_entities(msg); + } + } + if (RMW_RET_OK != rmw_destroy_wait_set(wait_set)) { + TERMINATE("failed to destroy wait set"); + } + } +} diff --git a/rmw_fastrtps_cpp/src/listener_thread.hpp b/rmw_fastrtps_cpp/src/listener_thread.hpp new file mode 100644 index 000000000..f6b2cab9e --- /dev/null +++ b/rmw_fastrtps_cpp/src/listener_thread.hpp @@ -0,0 +1,30 @@ +// 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 LISTENER_THREAD_HPP_ +#define LISTENER_THREAD_HPP_ + +#include "rmw/init.h" + +namespace rmw_fastrtps_cpp +{ + +rmw_ret_t +run_listener_thread(rmw_context_t * context); + +rmw_ret_t +join_listener_thread(rmw_context_t * context); + +} // namespace rmw_fastrtps_cpp +#endif // LISTENER_THREAD_HPP_ diff --git a/rmw_fastrtps_cpp/src/node_entities_info__type_support.cpp b/rmw_fastrtps_cpp/src/node_entities_info__type_support.cpp new file mode 100644 index 000000000..b068dc273 --- /dev/null +++ b/rmw_fastrtps_cpp/src/node_entities_info__type_support.cpp @@ -0,0 +1,52 @@ +// 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 "rosidl_generator_c/message_type_support_struct.h" +// rosidl_typesupport_cpp/message_type_support.hpp is installed by rosidl_generator_cpp +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rmw_dds_common/msg/node_entities_info__struct.hpp" +#include "rmw_dds_common/msg/node_entities_info__rosidl_typesupport_fastrtps_cpp.hpp" +#include "rmw_fastrtps_cpp/visibility_control.h" + +namespace rosidl_typesupport_cpp +{ + +template<> +RMW_FASTRTPS_CPP_PUBLIC +const rosidl_message_type_support_t * +get_message_type_support_handle() +{ + return ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_fastrtps_cpp, rmw_dds_common, msg, NodeEntitiesInfo)(); +} + +#ifdef __cplusplus +extern "C" +{ +#endif + +RMW_FASTRTPS_CPP_PUBLIC +const rosidl_message_type_support_t * +ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_cpp, rmw_dds_common, msg, NodeEntitiesInfo)() +{ + return get_message_type_support_handle(); +} + +#ifdef __cplusplus +} +#endif +} // namespace rosidl_typesupport_cpp diff --git a/rmw_fastrtps_cpp/src/participant_entities_info__type_support.cpp b/rmw_fastrtps_cpp/src/participant_entities_info__type_support.cpp new file mode 100644 index 000000000..ce27b9ad6 --- /dev/null +++ b/rmw_fastrtps_cpp/src/participant_entities_info__type_support.cpp @@ -0,0 +1,52 @@ +// 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 "rosidl_generator_c/message_type_support_struct.h" +// rosidl_typesupport_cpp/message_type_support.hpp is installed by rosidl_generator_cpp +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rmw_dds_common/msg/participant_entities_info__struct.hpp" +#include "rmw_dds_common/msg/participant_entities_info__rosidl_typesupport_fastrtps_cpp.hpp" +#include "rmw_fastrtps_cpp/visibility_control.h" + +namespace rosidl_typesupport_cpp +{ + +template<> +RMW_FASTRTPS_CPP_PUBLIC +const rosidl_message_type_support_t * +get_message_type_support_handle() +{ + return ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_fastrtps_cpp, rmw_dds_common, msg, ParticipantEntitiesInfo)(); +} + +#ifdef __cplusplus +extern "C" +{ +#endif + +RMW_FASTRTPS_CPP_PUBLIC +const rosidl_message_type_support_t * +ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_cpp, rmw_dds_common, msg, ParticipantEntitiesInfo)() +{ + return get_message_type_support_handle(); +} + +#ifdef __cplusplus +} +#endif +} // namespace rosidl_typesupport_cpp diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp new file mode 100644 index 000000000..c2fc1b642 --- /dev/null +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -0,0 +1,182 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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 "fastrtps/Domain.h" +#include "fastrtps/participant/Participant.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_cpp/identifier.hpp" +#include "rmw_fastrtps_cpp/publisher.hpp" + +#include "type_support_common.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; + +rmw_publisher_t * +rmw_fastrtps_cpp::create_publisher( + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_publisher_options_t * publisher_options, + bool keyed, + bool create_publisher_listener) +{ + if (!participant_info) { + RMW_SET_ERROR_MSG("participant_info is null"); + return nullptr; + } + if (!topic_name || strlen(topic_name) == 0) { + RMW_SET_ERROR_MSG("publisher topic is null or empty string"); + return nullptr; + } + if (!qos_policies) { + RMW_SET_ERROR_MSG("qos_policies is null"); + return nullptr; + } + if (!publisher_options) { + RMW_SET_ERROR_MSG("publisher_options is null"); + return nullptr; + } + + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); + if (!type_support) { + type_support = get_message_typesupport_handle( + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); + if (!type_support) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return nullptr; + } + } + + CustomPublisherInfo * info = nullptr; + rmw_publisher_t * rmw_publisher = nullptr; + eprosima::fastrtps::PublisherAttributes publisherParam; + + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + + // Load default XML profile. + Domain::getDefaultPublisherAttributes(publisherParam); + + // TODO(karsten1987) Verify consequences for std::unique_ptr? + info = new (std::nothrow) CustomPublisherInfo(); + if (!info) { + RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); + return nullptr; + } + + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_support->data; + + auto callbacks = static_cast(type_support->data); + std::string type_name = _create_type_name(callbacks); + if ( + !Domain::getRegisteredType( + participant_info->participant, type_name.c_str(), + reinterpret_cast(&info->type_support_))) + { + info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); + if (!info->type_support_) { + RMW_SET_ERROR_MSG("Failed to allocate MessageTypeSupport"); + goto fail; + } + _register_type(participant_info->participant, info->type_support_); + } + + if (!participant_info->leave_middleware_default_qos) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + publisherParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + publisherParam.topic.topicKind = + keyed ? eprosima::fastrtps::rtps::WITH_KEY : eprosima::fastrtps::rtps::NO_KEY; + publisherParam.topic.topicDataType = type_name; + publisherParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); + + if (!get_datawriter_qos(*qos_policies, publisherParam)) { + RMW_SET_ERROR_MSG("failed to get datawriter qos"); + goto fail; + } + + info->listener_ = nullptr; + if (create_publisher_listener) { + info->listener_ = new (std::nothrow) PubListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); + goto fail; + } + } + + info->publisher_ = Domain::createPublisher( + participant_info->participant, + publisherParam, + info->listener_); + if (!info->publisher_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); + goto fail; + } + + info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->publisher_->getGuid()); + + rmw_publisher = rmw_publisher_allocate(); + if (!rmw_publisher) { + RMW_SET_ERROR_MSG("failed to allocate publisher"); + goto fail; + } + rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; + rmw_publisher->data = info; + rmw_publisher->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + + if (!rmw_publisher->topic_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); + goto fail; + } + + memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); + + rmw_publisher->options = *publisher_options; + + return rmw_publisher; + +fail: + if (info) { + delete info->type_support_; + delete info->listener_; + delete info; + } + rmw_publisher_free(rmw_publisher); + + return nullptr; +} diff --git a/rmw_fastrtps_cpp/src/register_node.cpp b/rmw_fastrtps_cpp/src/register_node.cpp new file mode 100644 index 000000000..89a22b096 --- /dev/null +++ b/rmw_fastrtps_cpp/src/register_node.cpp @@ -0,0 +1,217 @@ +// 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 "rmw_fastrtps_cpp/register_node.hpp" + +#include +#include + +#include "rmw/error_handling.h" +#include "rmw/init.h" +#include "rmw/qos_profiles.h" + +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" + +#include "rmw_fastrtps_cpp/identifier.hpp" +#include "rmw_fastrtps_cpp/publisher.hpp" +#include "rmw_fastrtps_cpp/subscription.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/participant.hpp" +#include "rmw_fastrtps_shared_cpp/publisher.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +#include "rosidl_typesupport_cpp/message_type_support.hpp" + +#include "listener_thread.hpp" + +using rmw_dds_common::msg::ParticipantEntitiesInfo; + +static +rmw_ret_t +init_context_impl(rmw_context_t * context) +{ + rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); + rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); + + // This is currently not implemented in fastrtps + subscription_options.ignore_local_publications = true; + + std::unique_ptr common_context( + new(std::nothrow) rmw_dds_common::Context()); + if (!common_context) { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + participant_info( + rmw_fastrtps_shared_cpp::create_participant( + eprosima_fastrtps_identifier, + context->options.domain_id, + &context->options.security_options, + (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, + common_context.get()), + [&](CustomParticipantInfo * participant_info) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { + fprintf( + stderr, "Failed to destroy participant after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!participant_info) { + return RMW_RET_BAD_ALLOC; + } + + rmw_qos_profile_t qos = rmw_qos_profile_default; + + qos.avoid_ros_namespace_conventions = false; // Change this to true after testing + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos.depth = 1; + qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + std::unique_ptr> + publisher( + rmw_fastrtps_cpp::create_publisher( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "_participant_info", + &qos, + &publisher_options, + false, // our fastrtps typesupport doesn't support keyed topics + true), + [&](rmw_publisher_t * pub) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( + eprosima_fastrtps_identifier, + participant_info.get(), + pub)) + { + fprintf( + stderr, "Failed to destroy publisher after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!publisher) { + return RMW_RET_BAD_ALLOC; + } + + // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + std::unique_ptr> + subscription( + rmw_fastrtps_cpp::create_subscription( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "_participant_info", + &qos, + &subscription_options, + false, // our fastrtps typesupport doesn't support keyed topics + true), + [&](rmw_subscription_t * sub) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( + eprosima_fastrtps_identifier, + participant_info.get(), + sub)) + { + fprintf( + stderr, "Failed to destroy subscription after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!subscription) { + return RMW_RET_BAD_ALLOC; + } + + common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, participant_info->participant->getGuid()); + common_context->pub = publisher.get(); + common_context->sub = subscription.get(); + + context->impl->common = common_context.get(); + context->impl->participant_info = participant_info.get(); + + if (RMW_RET_OK != rmw_fastrtps_cpp::run_listener_thread(context)) { + return RMW_RET_ERROR; + } + common_context->graph_cache.add_participant(common_context->gid); + + publisher.release(); + subscription.release(); + common_context.release(); + participant_info.release(); + return RMW_RET_OK; +} + +rmw_ret_t +rmw_fastrtps_cpp::register_node(rmw_context_t * context) +{ + assert(context); + assert(context->impl); + + std::lock_guard guard(context->impl->mutex); + + if (!context->impl->count) { + rmw_ret_t ret = init_context_impl(context); + if (RMW_RET_OK != ret) { + return ret; + } + } + context->impl->count++; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_fastrtps_cpp::unregister_node(rmw_context_t * context) +{ + assert(context); + assert(context->impl); + + std::lock_guard guard(context->impl->mutex); + if (0u == --context->impl->count) { + rmw_ret_t ret; + ret = rmw_fastrtps_cpp::join_listener_thread(context); + if (RMW_RET_OK != ret) { + return ret; + } + + auto common_context = static_cast(context->impl->common); + auto participant_info = static_cast(context->impl->participant_info); + + common_context->graph_cache.remove_participant(common_context->gid); + ret = rmw_fastrtps_shared_cpp::destroy_subscription( + eprosima_fastrtps_identifier, + participant_info, + common_context->sub); + if (RMW_RET_OK != ret) { + return ret; + } + ret = rmw_fastrtps_shared_cpp::destroy_publisher( + eprosima_fastrtps_identifier, + participant_info, + common_context->pub); + if (RMW_RET_OK != ret) { + return ret; + } + ret = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); + if (RMW_RET_OK != ret) { + return ret; + } + delete common_context; + context->impl->common = nullptr; + context->impl->participant_info = nullptr; + } + return RMW_RET_OK; +} diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index c8a1a85d1..6d2d53456 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -25,6 +25,7 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" @@ -62,13 +63,15 @@ rmw_create_client( return nullptr; } - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + if (!participant_info) { + RMW_SET_ERROR_MSG("participant info is null"); return nullptr; } - Participant * participant = impl->participant; + Participant * participant = participant_info->participant; if (!participant) { RMW_SET_ERROR_MSG("participant handle is null"); return nullptr; @@ -130,7 +133,7 @@ rmw_create_client( _register_type(participant, info->response_type_support_); } - if (!impl->leave_middleware_default_qos) { + if (!participant_info->leave_middleware_default_qos) { subscriberParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } @@ -140,7 +143,7 @@ rmw_create_client( subscriberParam.topic.topicName = _create_topic_name( qos_policies, ros_service_response_prefix, service_name, "Reply"); - if (!impl->leave_middleware_default_qos) { + if (!participant_info->leave_middleware_default_qos) { publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; publisherParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; @@ -206,15 +209,54 @@ rmw_create_client( } memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_publisher_->getGuid()); + common_context->graph_cache.associate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( + gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + goto fail; + } + } + return rmw_client; fail: if (info != nullptr) { if (info->request_publisher_ != nullptr) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_publisher_->getGuid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removePublisher(info->request_publisher_); } if (info->response_subscriber_ != nullptr) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); + common_context->graph_cache.dissociate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removeSubscriber(info->response_subscriber_); } @@ -226,7 +268,7 @@ rmw_create_client( delete info->listener_; } - if (impl) { + if (participant_info) { if (info->request_type_support_ != nullptr) { rmw_fastrtps_shared_cpp::_unregister_type(participant, info->request_type_support_); } diff --git a/rmw_fastrtps_cpp/src/rmw_init.cpp b/rmw_fastrtps_cpp/src/rmw_init.cpp index 4334babea..36ab04176 100644 --- a/rmw_fastrtps_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_cpp/src/rmw_init.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -12,10 +13,33 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + +#include "rcutils/strdup.h" +#include "rcutils/types.h" + +#include "rmw/init.h" #include "rmw/impl/cpp/macros.hpp" +#include "rmw/init_options.h" +#include "rmw/publisher_options.h" #include "rmw/rmw.h" +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/participant.hpp" +#include "rmw_fastrtps_shared_cpp/publisher.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.hpp" + +#include "rosidl_typesupport_cpp/message_type_support.hpp" + #include "rmw_fastrtps_cpp/identifier.hpp" +#include "rmw_fastrtps_cpp/publisher.hpp" +#include "rmw_fastrtps_cpp/subscription.hpp" + +#include "listener_thread.hpp" extern "C" { @@ -32,6 +56,17 @@ rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t all init_options->implementation_identifier = eprosima_fastrtps_identifier; init_options->allocator = allocator; init_options->impl = nullptr; + init_options->name = rcutils_strdup("", allocator); + if (!init_options->name) { + RMW_SET_ERROR_MSG("failed to copy context name"); + return RMW_RET_BAD_ALLOC; + } + init_options->namespace_ = rcutils_strdup("", allocator); + if (!init_options->namespace_) { + allocator.deallocate(init_options->name, allocator.state); + RMW_SET_ERROR_MSG("failed to copy context namespace"); + return RMW_RET_BAD_ALLOC; + } return RMW_RET_OK; } @@ -49,27 +84,62 @@ rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) RMW_SET_ERROR_MSG("expected zero-initialized dst"); return RMW_RET_INVALID_ARGUMENT; } + const rcutils_allocator_t * allocator = &src->allocator; + rmw_ret_t ret = RMW_RET_OK; + *dst = *src; - return RMW_RET_OK; + dst->name = NULL; + dst->namespace_ = NULL; + dst->security_options = rmw_get_zero_initialized_security_options(); + + dst->name = rcutils_strdup(src->name, *allocator); + if (!dst->name) { + ret = RMW_RET_BAD_ALLOC; + goto fail; + } + dst->namespace_ = rcutils_strdup(src->namespace_, *allocator); + if (!dst->namespace_) { + ret = RMW_RET_BAD_ALLOC; + goto fail; + } + return rmw_security_options_copy(&src->security_options, allocator, &dst->security_options); +fail: + allocator->deallocate(dst->name, allocator->state); + allocator->deallocate(dst->namespace_, allocator->state); + return ret; } rmw_ret_t rmw_init_options_fini(rmw_init_options_t * init_options) { RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR(&(init_options->allocator), return RMW_RET_INVALID_ARGUMENT); + rcutils_allocator_t & allocator = init_options->allocator; + RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( init_options, init_options->implementation_identifier, eprosima_fastrtps_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + allocator.deallocate(init_options->name, allocator.state); + allocator.deallocate(init_options->namespace_, allocator.state); + rmw_security_options_fini(&init_options->security_options, &allocator); *init_options = rmw_get_zero_initialized_init_options(); return RMW_RET_OK; } +using rmw_dds_common::msg::ParticipantEntitiesInfo; + rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) { + std::unique_ptr clean_when_fail( + context, + [](rmw_context_t * context) + { + *context = rmw_get_zero_initialized_context(); + }); + rmw_ret_t ret = RMW_RET_OK; + RCUTILS_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( @@ -79,7 +149,24 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); context->instance_id = options->instance_id; context->implementation_identifier = eprosima_fastrtps_identifier; - context->impl = nullptr; + + std::unique_ptr context_impl(new rmw_context_impl_t()); + if (!context_impl) { + RMW_SET_ERROR_MSG("failed to allocate context impl"); + return RMW_RET_BAD_ALLOC; + } + context->options = rmw_get_zero_initialized_init_options(); + ret = rmw_init_options_copy(options, &context->options); + if (RMW_RET_OK != ret) { + if (RMW_RET_OK != rmw_init_options_fini(&context->options)) { + fprintf( + stderr, + "Failed to destroy init options after ':" RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + return ret; + } + context->impl = context_impl.release(); + clean_when_fail.release(); return RMW_RET_OK; } @@ -106,8 +193,7 @@ rmw_context_fini(rmw_context_t * context) context->implementation_identifier, eprosima_fastrtps_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - // context impl is explicitly supposed to be nullptr for now, see rmw_init's code - // RCUTILS_CHECK_ARGUMENT_FOR_NULL(context->impl, RMW_RET_INVALID_ARGUMENT); + delete context->impl; *context = rmw_get_zero_initialized_context(); return RMW_RET_OK; } diff --git a/rmw_fastrtps_cpp/src/rmw_node.cpp b/rmw_fastrtps_cpp/src/rmw_node.cpp index 5d7492d06..be8dc1236 100644 --- a/rmw_fastrtps_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -23,11 +24,13 @@ #include "rmw/allocators.h" #include "rmw/error_handling.h" #include "rmw/impl/cpp/macros.hpp" +#include "rmw/ret_types.h" #include "rmw/rmw.h" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" +#include "rmw_fastrtps_cpp/register_node.hpp" extern "C" { @@ -37,25 +40,37 @@ rmw_create_node( const char * name, const char * namespace_, size_t domain_id, - const rmw_node_security_options_t * security_options, + const rmw_security_options_t * security_options, bool localhost_only) { + (void)domain_id; + (void)security_options; + (void)localhost_only; RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( init context, context->implementation_identifier, eprosima_fastrtps_identifier, // TODO(wjwwood): replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when refactored - return NULL); + return nullptr); + + if (RMW_RET_OK != rmw_fastrtps_cpp::register_node(context)) { + return nullptr; + } + return rmw_fastrtps_shared_cpp::__rmw_create_node( - eprosima_fastrtps_identifier, name, namespace_, domain_id, security_options, localhost_only); + context, eprosima_fastrtps_identifier, name, namespace_); } rmw_ret_t rmw_destroy_node(rmw_node_t * node) { - return rmw_fastrtps_shared_cpp::__rmw_destroy_node( + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_node( eprosima_fastrtps_identifier, node); + if (RMW_RET_OK != ret) { + return ret; + } + return rmw_fastrtps_cpp::unregister_node(node->context); } rmw_ret_t diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index 66c1b7799..45557ec51 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -20,19 +21,14 @@ #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" -#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" -#include "rmw_fastrtps_shared_cpp/names.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" -#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" +#include "rmw_fastrtps_cpp/publisher.hpp" -#include "./type_support_common.hpp" - -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" extern "C" { @@ -77,163 +73,40 @@ rmw_create_publisher( return nullptr; } - if (!topic_name || strlen(topic_name) == 0) { - RMW_SET_ERROR_MSG("publisher topic is null or empty string"); - return nullptr; - } - - if (!qos_policies) { - RMW_SET_ERROR_MSG("qos_policies is null"); - return nullptr; - } - - if (!publisher_options) { - RMW_SET_ERROR_MSG("publisher_options is null"); - return nullptr; - } - - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return nullptr; - } - - Participant * participant = impl->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } - - const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); - if (!type_support) { - type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); - if (!type_support) { - RMW_SET_ERROR_MSG("type support not from this implementation"); - return nullptr; - } - } - - CustomPublisherInfo * info = nullptr; - rmw_publisher_t * rmw_publisher = nullptr; - eprosima::fastrtps::PublisherAttributes publisherParam; + rmw_publisher_t * publisher = rmw_fastrtps_cpp::create_publisher( + static_cast(node->context->impl->participant_info), + type_supports, + topic_name, + qos_policies, + publisher_options, + false, + true); - if (!is_valid_qos(*qos_policies)) { + if (!publisher) { return nullptr; } - // Load default XML profile. - Domain::getDefaultPublisherAttributes(publisherParam); + auto common_context = static_cast(node->context->impl->common); - // TODO(karsten1987) Verify consequences for std::unique_ptr? - info = new (std::nothrow) CustomPublisherInfo(); - if (!info) { - RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); - return nullptr; - } - - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support->data; - - auto callbacks = static_cast(type_support->data); - std::string type_name = _create_type_name(callbacks); - if ( - !Domain::getRegisteredType( - participant, type_name.c_str(), - reinterpret_cast(&info->type_support_))) + auto info = static_cast(publisher->data); { - info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("Failed to allocate MessageTypeSupport"); - goto fail; - } - _register_type(participant, info->type_support_); - } - - if (!impl->leave_middleware_default_qos) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - } - - publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - publisherParam.topic.topicDataType = type_name; - publisherParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); - - // 1 Heartbeat every 10ms - // publisherParam.times.heartbeatPeriod.seconds = 0; - // publisherParam.times.heartbeatPeriod.fraction = 42949673; - - // 300000 bytes each 10ms - // ThroughputControllerDescriptor throughputController{3000000, 10}; - // publisherParam.throughputController = throughputController; - - if (!get_datawriter_qos(*qos_policies, publisherParam)) { - RMW_SET_ERROR_MSG("failed to get datawriter qos"); - goto fail; - } - - info->listener_ = new (std::nothrow) PubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); - goto fail; - } - - info->publisher_ = Domain::createPublisher(participant, publisherParam, info->listener_); - if (!info->publisher_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); - goto fail; - } - - info->publisher_gid.implementation_identifier = eprosima_fastrtps_identifier; - static_assert( - sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, - "RMW_GID_STORAGE_SIZE insufficient to store the rmw_fastrtps_cpp GID implementation." - ); - - memset(info->publisher_gid.data, 0, RMW_GID_STORAGE_SIZE); - rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( - info->publisher_->getGuid(), - info->publisher_gid.data); - - rmw_publisher = rmw_publisher_allocate(); - if (!rmw_publisher) { - RMW_SET_ERROR_MSG("failed to allocate publisher"); - goto fail; - } - rmw_publisher->can_loan_messages = false; - rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; - rmw_publisher->data = info; - rmw_publisher->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); - - if (!rmw_publisher->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); - goto fail; - } - - memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); - - rmw_publisher->options = *publisher_options; - - return rmw_publisher; - -fail: - if (info) { - if (info->type_support_ != nullptr) { - delete info->type_support_; - } - if (info->listener_ != nullptr) { - delete info->listener_; + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_writer( + info->publisher_gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + rmw_fastrtps_shared_cpp::__rmw_destroy_publisher( + eprosima_fastrtps_identifier, node, publisher); + return nullptr; } - delete info; } - - if (rmw_publisher) { - rmw_publisher_free(rmw_publisher); - } - - return nullptr; + return publisher; } rmw_ret_t diff --git a/rmw_fastrtps_cpp/src/rmw_security.cpp b/rmw_fastrtps_cpp/src/rmw_security.cpp new file mode 100644 index 000000000..6774438dd --- /dev/null +++ b/rmw_fastrtps_cpp/src/rmw_security.cpp @@ -0,0 +1,24 @@ +// 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 "rmw/security.h" + +extern "C" +{ +bool +rmw_use_node_name_in_security_directory_lookup() +{ + return false; +} +} // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 70722d497..e1dbe4eac 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -36,10 +36,11 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" -#include "./type_support_common.hpp" +#include "type_support_common.hpp" using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; @@ -74,7 +75,9 @@ rmw_create_service( return nullptr; } - const CustomParticipantInfo * impl = static_cast(node->data); + const CustomParticipantInfo * impl = + static_cast(node->context->impl->participant_info); + auto common_context = static_cast(node->context->impl->common); if (!impl) { RMW_SET_ERROR_MSG("node impl is null"); return nullptr; @@ -144,7 +147,6 @@ rmw_create_service( subscriberParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; subscriberParam.topic.topicDataType = request_type_name; subscriberParam.topic.topicName = _create_topic_name( @@ -212,16 +214,55 @@ rmw_create_service( } memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_subscriber_->getGuid()); + common_context->graph_cache.associate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_writer( + gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + goto fail; + } + } + return rmw_service; fail: if (info) { if (info->response_publisher_) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removePublisher(info->response_publisher_); } if (info->request_subscriber_) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_subscriber_->getGuid()); + common_context->graph_cache.dissociate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removeSubscriber(info->request_subscriber_); } diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp index 590a77c85..3a1d884ee 100644 --- a/rmw_fastrtps_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_cpp/src/rmw_subscription.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -12,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include #include "rmw/allocators.h" #include "rmw/error_handling.h" @@ -21,21 +22,11 @@ #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" -#include "rmw_fastrtps_shared_cpp/names.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" -#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" - -#include "fastrtps/participant/Participant.h" -#include "fastrtps/subscriber/Subscriber.h" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" - -#include "./type_support_common.hpp" - -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; +#include "rmw_fastrtps_cpp/subscription.hpp" extern "C" { @@ -80,138 +71,40 @@ rmw_create_subscription( return nullptr; } - if (!topic_name || strlen(topic_name) == 0) { - RMW_SET_ERROR_MSG("subscription topic is null or empty string"); - return nullptr; - } - - if (!qos_policies) { - RMW_SET_ERROR_MSG("qos_profile is null"); - return nullptr; - } - - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return nullptr; - } - - Participant * participant = impl->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } - - const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); - if (!type_support) { - type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); - if (!type_support) { - RMW_SET_ERROR_MSG("type support not from this implementation"); - return nullptr; - } - } - - if (!is_valid_qos(*qos_policies)) { - return nullptr; - } - - CustomSubscriberInfo * info = nullptr; - rmw_subscription_t * rmw_subscription = nullptr; - eprosima::fastrtps::SubscriberAttributes subscriberParam; - - // Load default XML profile. - Domain::getDefaultSubscriberAttributes(subscriberParam); - - info = new (std::nothrow) CustomSubscriberInfo(); - if (!info) { - RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); + auto participant_info = + static_cast(node->context->impl->participant_info); + rmw_subscription_t * subscription = rmw_fastrtps_cpp::create_subscription( + participant_info, + type_supports, + topic_name, + qos_policies, + subscription_options, + false, // use no keyed topic + true); // create subscription listener + if (!subscription) { return nullptr; } - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support->data; - - auto callbacks = static_cast(type_support->data); - std::string type_name = _create_type_name(callbacks); - if ( - !Domain::getRegisteredType( - participant, type_name.c_str(), - reinterpret_cast(&info->type_support_))) + auto common_context = static_cast(node->context->impl->common); + auto info = static_cast(subscription->data); { - info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("failed to allocate MessageTypeSupport_cpp"); - goto fail; - } - _register_type(participant, info->type_support_); - } - - if (!impl->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - } - - subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - subscriberParam.topic.topicDataType = type_name; - subscriberParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); - - if (!get_datareader_qos(*qos_policies, subscriberParam)) { - RMW_SET_ERROR_MSG("failed to get datareader qos"); - goto fail; - } - - info->listener_ = new (std::nothrow) SubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); - goto fail; - } - - info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); - if (!info->subscriber_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); - goto fail; - } - - rmw_subscription = rmw_subscription_allocate(); - if (!rmw_subscription) { - RMW_SET_ERROR_MSG("failed to allocate subscription"); - goto fail; - } - rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; - rmw_subscription->data = info; - rmw_subscription->topic_name = - reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); - - if (!rmw_subscription->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); - goto fail; - } - - memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); - - rmw_subscription->options = *subscription_options; - rmw_subscription->can_loan_messages = false; - return rmw_subscription; - -fail: - - if (info != nullptr) { - if (info->type_support_ != nullptr) { - delete info->type_support_; - } - if (info->listener_ != nullptr) { - delete info->listener_; + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( + info->subscription_gid_, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( + eprosima_fastrtps_identifier, node, subscription); + return nullptr; } - delete info; } - - if (rmw_subscription) { - rmw_subscription_free(rmw_subscription); - } - - return nullptr; + return subscription; } rmw_ret_t diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp new file mode 100644 index 000000000..4e0941052 --- /dev/null +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -0,0 +1,173 @@ +// 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 "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "fastrtps/participant/Participant.h" +#include "fastrtps/subscriber/Subscriber.h" + +#include "rmw_fastrtps_cpp/identifier.hpp" +#include "rmw_fastrtps_cpp/subscription.hpp" + +#include "type_support_common.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; + + +namespace rmw_fastrtps_cpp +{ + +rmw_subscription_t * +create_subscription( + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_subscription_options_t * subscription_options, + bool keyed, + bool create_subscription_listener) +{ + if (!topic_name || strlen(topic_name) == 0) { + RMW_SET_ERROR_MSG("subscription topic is null or empty string"); + return nullptr; + } + if (!qos_policies) { + RMW_SET_ERROR_MSG("qos_policies is null"); + return nullptr; + } + if (!subscription_options) { + RMW_SET_ERROR_MSG("subscription_options is null"); + return nullptr; + } + if (!participant_info) { + RMW_SET_ERROR_MSG("participant_info is null"); + return nullptr; + } + Participant * participant = participant_info->participant; + if (!participant) { + RMW_SET_ERROR_MSG("participant handle is null"); + return nullptr; + } + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); + if (!type_support) { + type_support = get_message_typesupport_handle( + type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); + if (!type_support) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return nullptr; + } + } + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + CustomSubscriberInfo * info = nullptr; + rmw_subscription_t * rmw_subscription = nullptr; + eprosima::fastrtps::SubscriberAttributes subscriberParam; + + // Load default XML profile. + Domain::getDefaultSubscriberAttributes(subscriberParam); + info = new (std::nothrow) CustomSubscriberInfo(); + if (!info) { + RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); + return nullptr; + } + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_support->data; + + auto callbacks = static_cast(type_support->data); + std::string type_name = _create_type_name(callbacks); + if ( + !Domain::getRegisteredType( + participant, type_name.c_str(), + reinterpret_cast(&info->type_support_))) + { + info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); + if (!info->type_support_) { + RMW_SET_ERROR_MSG("failed to allocate MessageTypeSupport_cpp"); + goto fail; + } + _register_type(participant, info->type_support_); + } + if (!participant_info->leave_middleware_default_qos) { + subscriberParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + subscriberParam.topic.topicKind = + keyed ? eprosima::fastrtps::rtps::WITH_KEY : eprosima::fastrtps::rtps::NO_KEY; + subscriberParam.topic.topicDataType = type_name; + subscriberParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); + + if (!get_datareader_qos(*qos_policies, subscriberParam)) { + RMW_SET_ERROR_MSG("failed to get datareader qos"); + goto fail; + } + info->listener_ = nullptr; + if (create_subscription_listener) { + info->listener_ = new (std::nothrow) SubListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); + goto fail; + } + } + info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); + if (!info->subscriber_) { + RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); + goto fail; + } + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->subscriber_->getGuid()); + rmw_subscription = rmw_subscription_allocate(); + if (!rmw_subscription) { + RMW_SET_ERROR_MSG("failed to allocate subscription"); + goto fail; + } + rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; + rmw_subscription->data = info; + rmw_subscription->topic_name = + reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + if (!rmw_subscription->topic_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); + goto fail; + } + memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); + + rmw_subscription->options = *subscription_options; + return rmw_subscription; + +fail: + if (info != nullptr) { + delete info->type_support_; + delete info->listener_; + delete info; + } + rmw_subscription_free(rmw_subscription); + return nullptr; +} +} // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/test/test_gid_message.cpp b/rmw_fastrtps_cpp/test/test_gid_message.cpp new file mode 100644 index 000000000..f6f30c53a --- /dev/null +++ b/rmw_fastrtps_cpp/test/test_gid_message.cpp @@ -0,0 +1,42 @@ +// 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 "rmw_dds_common/msg/gid.hpp" + +#include "rosidl_generator_c/message_type_support_struct.h" +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rosidl_typesupport_fastrtps_cpp/identifier.hpp" + +using rmw_dds_common::msg::Gid; + + +TEST(Test_gid, constructor_destructor) { + Gid gid; + ASSERT_EQ(24u, gid.data.size()); + gid.data[0] = 'a'; +} + +TEST(Test_gid, get_typesupport) { + const rosidl_message_type_support_t * type_support_1 = + rosidl_typesupport_cpp::get_message_type_support_handle(); + const rosidl_message_type_support_t * type_support_2 = get_message_typesupport_handle( + type_support_1, rosidl_typesupport_fastrtps_cpp::typesupport_identifier); + + ASSERT_EQ(type_support_1->typesupport_identifier, type_support_2->typesupport_identifier); + ASSERT_EQ(type_support_1->data, type_support_2->data); + ASSERT_EQ(type_support_1->func, type_support_2->func); +} diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index 7c5305851..1cab6e08b 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -33,6 +33,7 @@ find_package(ament_cmake_ros REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) +find_package(rmw_dds_common REQUIRED) find_package(rmw_fastrtps_shared_cpp REQUIRED) find_package(fastrtps_cmake_module REQUIRED) @@ -56,8 +57,13 @@ add_library(rmw_fastrtps_dynamic_cpp src/get_publisher.cpp src/get_service.cpp src/get_subscriber.cpp + src/gid__type_support.cpp src/identifier.cpp - src/rmw_logging.cpp + src/listener_thread.cpp + src/node_entities_info__type_support.cpp + src/participant_entities_info__type_support.cpp + src/publisher.cpp + src/register_node.cpp src/rmw_client.cpp src/rmw_compare_gids_equal.cpp src/rmw_count.cpp @@ -68,6 +74,7 @@ add_library(rmw_fastrtps_dynamic_cpp src/rmw_get_topic_endpoint_info.cpp src/rmw_guard_condition.cpp src/rmw_init.cpp + src/rmw_logging.cpp src/rmw_node.cpp src/rmw_node_info_and_types.cpp src/rmw_node_names.cpp @@ -75,6 +82,7 @@ add_library(rmw_fastrtps_dynamic_cpp src/rmw_publisher.cpp src/rmw_request.cpp src/rmw_response.cpp + src/rmw_security.cpp src/rmw_serialize.cpp src/rmw_service.cpp src/rmw_service_names_and_types.cpp @@ -85,10 +93,11 @@ add_library(rmw_fastrtps_dynamic_cpp src/rmw_trigger_guard_condition.cpp src/rmw_wait.cpp src/rmw_wait_set.cpp + src/serialization_format.cpp + src/subscription.cpp src/type_support_common.cpp src/type_support_proxy.cpp src/type_support_registry.cpp - src/serialization_format.cpp ) target_link_libraries(rmw_fastrtps_dynamic_cpp fastcdr fastrtps) @@ -101,6 +110,7 @@ ament_target_dependencies(rmw_fastrtps_dynamic_cpp "rosidl_typesupport_fastrtps_cpp" "rosidl_typesupport_introspection_c" "rosidl_typesupport_introspection_cpp" + "rmw_dds_common" "rmw_fastrtps_shared_cpp" "rmw" "rosidl_generator_c" diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp new file mode 100644 index 000000000..4f98b6c14 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/publisher.hpp @@ -0,0 +1,36 @@ +// Copyright 2017-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_FASTRTPS_DYNAMIC_CPP__PUBLISHER_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__PUBLISHER_HPP_ + +#include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +rmw_publisher_t * +create_publisher( + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_publisher_options_t * publisher_options, + bool keyed, + bool create_publisher_listener); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__PUBLISHER_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/register_node.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/register_node.hpp new file mode 100644 index 000000000..5abde1349 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/register_node.hpp @@ -0,0 +1,41 @@ +// 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_FASTRTPS_DYNAMIC_CPP__REGISTER_NODE_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__REGISTER_NODE_HPP_ + +#include "rmw/init.h" +#include "rmw/types.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +/// Register node in context. +/** + * Function that should be called when creating a node, + * before using `context->impl`. + */ +rmw_ret_t +register_node(rmw_context_t * context); + +/// Unregister node in context. +/** + * Function that should be called when destroying a node. + */ +rmw_ret_t +unregister_node(rmw_context_t * context); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__REGISTER_NODE_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp new file mode 100644 index 000000000..0db879040 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/subscription.hpp @@ -0,0 +1,38 @@ +// 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_FASTRTPS_DYNAMIC_CPP__SUBSCRIPTION_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__SUBSCRIPTION_HPP_ + +#include "rmw/rmw.h" +#include "rmw/subscription_options.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +rmw_subscription_t * +create_subscription( + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_subscription_options_t * subscription_options, + bool keyed, + bool create_subscription_listener); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__SUBSCRIPTION_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/package.xml b/rmw_fastrtps_dynamic_cpp/package.xml index 5dfb470bf..cfc49d436 100644 --- a/rmw_fastrtps_dynamic_cpp/package.xml +++ b/rmw_fastrtps_dynamic_cpp/package.xml @@ -19,6 +19,7 @@ rcpputils rcutils rmw + rmw_dds_common rmw_fastrtps_shared_cpp rosidl_generator_c rosidl_typesupport_fastrtps_c @@ -32,6 +33,7 @@ rcpputils rcutils rmw + rmw_dds_common rmw_fastrtps_shared_cpp rosidl_generator_c rosidl_typesupport_fastrtps_c diff --git a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp index ba7aa5c5e..a1c2107de 100644 --- a/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/get_participant.cpp @@ -15,6 +15,7 @@ #include "rmw_fastrtps_dynamic_cpp/get_participant.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" namespace rmw_fastrtps_dynamic_cpp @@ -29,7 +30,7 @@ get_participant(rmw_node_t * node) if (node->implementation_identifier != eprosima_fastrtps_identifier) { return nullptr; } - auto impl = static_cast(node->data); + auto impl = static_cast(node->context->impl->participant_info); return impl->participant; } diff --git a/rmw_fastrtps_dynamic_cpp/src/gid__type_support.cpp b/rmw_fastrtps_dynamic_cpp/src/gid__type_support.cpp new file mode 100644 index 000000000..2e54e1d69 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/gid__type_support.cpp @@ -0,0 +1,52 @@ +// 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 "rosidl_generator_c/message_type_support_struct.h" +// rosidl_typesupport_cpp/message_type_support.hpp is installed by rosidl_generator_cpp +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rmw_dds_common/msg/gid__struct.hpp" +#include "rmw_dds_common/msg/gid__rosidl_typesupport_introspection_cpp.hpp" +#include "rmw_fastrtps_dynamic_cpp/visibility_control.h" + +namespace rosidl_typesupport_cpp +{ + +template<> +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +const rosidl_message_type_support_t * +get_message_type_support_handle() +{ + return ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_introspection_cpp, rmw_dds_common, msg, Gid)(); +} + +#ifdef __cplusplus +extern "C" +{ +#endif + +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +const rosidl_message_type_support_t * +ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_cpp, rmw_dds_common, msg, Gid)() +{ + return get_message_type_support_handle(); +} + +#ifdef __cplusplus +} +#endif +} // namespace rosidl_typesupport_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp b/rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp new file mode 100644 index 000000000..34c3208a3 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp @@ -0,0 +1,170 @@ +// 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 "rcutils/macros.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/init.h" +#include "rmw/ret_types.h" +#include "rmw/rmw.h" +#include "rmw/types.h" +#include "rmw/impl/cpp/macros.hpp" + +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/gid_utils.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" + +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +#include "listener_thread.hpp" + +using rmw_dds_common::operator<<; + +static +void +node_listener(rmw_context_t * context); + +rmw_ret_t +rmw_fastrtps_dynamic_cpp::run_listener_thread(rmw_context_t * context) +{ + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.store(true); + common_context->listener_thread_gc = rmw_create_guard_condition(context); + if (common_context->listener_thread_gc) { + try { + common_context->listener_thread = std::thread(node_listener, context); + return RMW_RET_OK; + } catch (const std::exception & exc) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to create std::thread: %s", exc.what()); + } catch (...) { + RMW_SET_ERROR_MSG("Failed to create std::thread"); + } + } else { + RMW_SET_ERROR_MSG("Failed to create guard condition"); + } + common_context->thread_is_running.store(false); + if (common_context->listener_thread_gc) { + if (RMW_RET_OK != rmw_destroy_guard_condition(common_context->listener_thread_gc)) { + fprintf( + stderr, + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" + RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); + } + } + return RMW_RET_ERROR; +} + +rmw_ret_t +rmw_fastrtps_dynamic_cpp::join_listener_thread(rmw_context_t * context) +{ + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.exchange(false); + rmw_ret_t rmw_ret = rmw_trigger_guard_condition(common_context->listener_thread_gc); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + try { + common_context->listener_thread.join(); + } catch (const std::exception & exc) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to join std::thread: %s", exc.what()); + return RMW_RET_ERROR; + } catch (...) { + RMW_SET_ERROR_MSG("Failed to join std::thread"); + return RMW_RET_ERROR; + } + rmw_ret = rmw_destroy_guard_condition(common_context->listener_thread_gc); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + return RMW_RET_OK; +} + +#define TERMINATE(msg) \ + do { \ + fprintf( \ + stderr, \ + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ + RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) ": %s, terminating ...", \ + rmw_get_error_string().str); \ + std::terminate(); \ + } while (0) + +void +node_listener(rmw_context_t * context) +{ + assert(nullptr != context); + assert(nullptr != context->impl); + assert(nullptr != context->impl->common); + auto common_context = static_cast(context->impl->common); + while (common_context->thread_is_running.load()) { + assert(nullptr != common_context->sub); + assert(nullptr != common_context->sub->data); + void * subscriptions_buffer[] = {common_context->sub->data}; + void * guard_conditions_buffer[] = {common_context->listener_thread_gc->data}; + rmw_subscriptions_t subscriptions; + rmw_guard_conditions_t guard_conditions; + subscriptions.subscriber_count = 1; + subscriptions.subscribers = subscriptions_buffer; + guard_conditions.guard_condition_count = 1; + guard_conditions.guard_conditions = guard_conditions_buffer; + // number of conditions of a subscription is 2 + rmw_wait_set_t * wait_set = rmw_create_wait_set(context, 2); + if (nullptr == wait_set) { + TERMINATE("failed to create wait set"); + } + if (RMW_RET_OK != rmw_wait( + &subscriptions, + &guard_conditions, + nullptr, + nullptr, + nullptr, + wait_set, + nullptr)) + { + TERMINATE("rmw_wait failed"); + } + if (subscriptions_buffer[0]) { + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + bool taken; + if (RMW_RET_OK != rmw_take( + common_context->sub, + static_cast(&msg), + &taken, + nullptr)) + { + TERMINATE("rmw_take failed"); + } + if (taken) { + if (std::memcmp( + reinterpret_cast(common_context->gid.data), + reinterpret_cast(&msg.gid.data), + RMW_GID_STORAGE_SIZE) == 0) + { + // ignore local messages + continue; + } + common_context->graph_cache.update_participant_entities(msg); + } + } + if (RMW_RET_OK != rmw_destroy_wait_set(wait_set)) { + TERMINATE("failed to destroy wait set"); + } + } +} diff --git a/rmw_fastrtps_dynamic_cpp/src/listener_thread.hpp b/rmw_fastrtps_dynamic_cpp/src/listener_thread.hpp new file mode 100644 index 000000000..f66644788 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/listener_thread.hpp @@ -0,0 +1,30 @@ +// 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 LISTENER_THREAD_HPP_ +#define LISTENER_THREAD_HPP_ + +#include "rmw/init.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +rmw_ret_t +run_listener_thread(rmw_context_t * context); + +rmw_ret_t +join_listener_thread(rmw_context_t * context); + +} // namespace rmw_fastrtps_dynamic_cpp +#endif // LISTENER_THREAD_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/src/node_entities_info__type_support.cpp b/rmw_fastrtps_dynamic_cpp/src/node_entities_info__type_support.cpp new file mode 100644 index 000000000..d03555f01 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/node_entities_info__type_support.cpp @@ -0,0 +1,52 @@ +// 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 "rosidl_generator_c/message_type_support_struct.h" +// rosidl_typesupport_cpp/message_type_support.hpp is installed by rosidl_generator_cpp +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rmw_dds_common/msg/node_entities_info__struct.hpp" +#include "rmw_dds_common/msg/node_entities_info__rosidl_typesupport_introspection_cpp.hpp" +#include "rmw_fastrtps_dynamic_cpp/visibility_control.h" + +namespace rosidl_typesupport_cpp +{ + +template<> +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +const rosidl_message_type_support_t * +get_message_type_support_handle() +{ + return ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_introspection_cpp, rmw_dds_common, msg, NodeEntitiesInfo)(); +} + +#ifdef __cplusplus +extern "C" +{ +#endif + +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +const rosidl_message_type_support_t * +ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_cpp, rmw_dds_common, msg, NodeEntitiesInfo)() +{ + return get_message_type_support_handle(); +} + +#ifdef __cplusplus +} +#endif +} // namespace rosidl_typesupport_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/participant_entities_info__type_support.cpp b/rmw_fastrtps_dynamic_cpp/src/participant_entities_info__type_support.cpp new file mode 100644 index 000000000..c9d5cb3bf --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/participant_entities_info__type_support.cpp @@ -0,0 +1,52 @@ +// 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 "rosidl_generator_c/message_type_support_struct.h" +// rosidl_typesupport_cpp/message_type_support.hpp is installed by rosidl_generator_cpp +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rmw_dds_common/msg/participant_entities_info__struct.hpp" +#include "rmw_dds_common/msg/participant_entities_info__rosidl_typesupport_introspection_cpp.hpp" +#include "rmw_fastrtps_dynamic_cpp/visibility_control.h" + +namespace rosidl_typesupport_cpp +{ + +template<> +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +const rosidl_message_type_support_t * +get_message_type_support_handle() +{ + return ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_introspection_cpp, rmw_dds_common, msg, ParticipantEntitiesInfo)(); +} + +#ifdef __cplusplus +extern "C" +{ +#endif + +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +const rosidl_message_type_support_t * +ROSIDL_TYPESUPPORT_INTERFACE__MESSAGE_SYMBOL_NAME( + rosidl_typesupport_cpp, rmw_dds_common, msg, ParticipantEntitiesInfo)() +{ + return get_message_type_support_handle(); +} + +#ifdef __cplusplus +} +#endif +} // namespace rosidl_typesupport_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp new file mode 100644 index 000000000..8bfb28923 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -0,0 +1,216 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/publisher.hpp" + +#include "type_support_common.hpp" +#include "type_support_registry.hpp" + +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; +using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; + +rmw_publisher_t * +rmw_fastrtps_dynamic_cpp::create_publisher( + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_publisher_options_t * publisher_options, + bool keyed, + bool create_publisher_listener) +{ + (void)keyed; + (void)create_publisher_listener; + + if (!participant_info) { + RMW_SET_ERROR_MSG("participant_info is null"); + return nullptr; + } + + if (!topic_name || strlen(topic_name) == 0) { + RMW_SET_ERROR_MSG("publisher topic is null or empty string"); + return nullptr; + } + + if (!qos_policies) { + RMW_SET_ERROR_MSG("qos_policies is null"); + return nullptr; + } + + if (!publisher_options) { + RMW_SET_ERROR_MSG("publisher_options is null"); + return nullptr; + } + + Participant * participant = participant_info->participant; + if (!participant) { + RMW_SET_ERROR_MSG("participant handle is null"); + return nullptr; + } + + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return nullptr; + } + } + + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + + CustomPublisherInfo * info = nullptr; + rmw_publisher_t * rmw_publisher = nullptr; + eprosima::fastrtps::PublisherAttributes publisherParam; + const eprosima::fastrtps::rtps::GUID_t * guid = nullptr; + + // Load default XML profile. + Domain::getDefaultPublisherAttributes(publisherParam); + + // TODO(karsten1987) Verify consequences for std::unique_ptr? + info = new (std::nothrow) CustomPublisherInfo(); + if (!info) { + RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); + return nullptr; + } + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto type_impl = type_registry.get_message_type_support(type_support); + if (!type_impl) { + delete info; + RMW_SET_ERROR_MSG("failed to allocate type support"); + return nullptr; + } + + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_impl; + + std::string type_name = _create_type_name( + type_support->data, info->typesupport_identifier_); + if ( + !Domain::getRegisteredType( + participant, type_name.c_str(), + reinterpret_cast(&info->type_support_))) + { + info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); + if (!info->type_support_) { + RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); + goto fail; + } + _register_type(participant, info->type_support_); + } + + if (!participant_info->leave_middleware_default_qos) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + publisherParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; + publisherParam.topic.topicDataType = type_name; + publisherParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); + + // 1 Heartbeat every 10ms + // publisherParam.times.heartbeatPeriod.seconds = 0; + // publisherParam.times.heartbeatPeriod.fraction = 42949673; + + // 300000 bytes each 10ms + // ThroughputControllerDescriptor throughputController{3000000, 10}; + // publisherParam.throughputController = throughputController; + + if (!get_datawriter_qos(*qos_policies, publisherParam)) { + RMW_SET_ERROR_MSG("failed to get datawriter qos"); + goto fail; + } + + info->listener_ = new (std::nothrow) PubListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); + goto fail; + } + + info->publisher_ = Domain::createPublisher(participant, publisherParam, info->listener_); + if (!info->publisher_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); + goto fail; + } + + info->publisher_gid.implementation_identifier = eprosima_fastrtps_identifier; + static_assert( + sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, + "RMW_GID_STORAGE_SIZE insufficient to store the rmw_fastrtps_dynamic_cpp GID implementation." + ); + + memset(info->publisher_gid.data, 0, RMW_GID_STORAGE_SIZE); + guid = &info->publisher_->getGuid(); + if (!guid) { + RMW_SET_ERROR_MSG("no guid found for publisher"); + goto fail; + } + memcpy(info->publisher_gid.data, guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); + + rmw_publisher = rmw_publisher_allocate(); + if (!rmw_publisher) { + RMW_SET_ERROR_MSG("failed to allocate publisher"); + goto fail; + } + rmw_publisher->can_loan_messages = false; + rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; + rmw_publisher->data = info; + rmw_publisher->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + + if (!rmw_publisher->topic_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); + goto fail; + } + + memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); + + rmw_publisher->options = *publisher_options; + + return rmw_publisher; + +fail: + if (info) { + delete info->type_support_; + delete info->listener_; + delete info; + } + type_registry.return_message_type_support(type_support); + rmw_publisher_free(rmw_publisher); + + return nullptr; +} diff --git a/rmw_fastrtps_dynamic_cpp/src/register_node.cpp b/rmw_fastrtps_dynamic_cpp/src/register_node.cpp new file mode 100644 index 000000000..6ab88330a --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/register_node.cpp @@ -0,0 +1,217 @@ +// 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 "rmw_fastrtps_dynamic_cpp/register_node.hpp" + +#include +#include + +#include "rmw/error_handling.h" +#include "rmw/init.h" +#include "rmw/qos_profiles.h" + +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/publisher.hpp" +#include "rmw_fastrtps_dynamic_cpp/subscription.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/participant.hpp" +#include "rmw_fastrtps_shared_cpp/publisher.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +#include "rosidl_typesupport_cpp/message_type_support.hpp" + +#include "listener_thread.hpp" + +using rmw_dds_common::msg::ParticipantEntitiesInfo; + +static +rmw_ret_t +init_context_impl(rmw_context_t * context) +{ + rmw_publisher_options_t publisher_options = rmw_get_default_publisher_options(); + rmw_subscription_options_t subscription_options = rmw_get_default_subscription_options(); + + // This is currently not implemented in fastrtps + subscription_options.ignore_local_publications = true; + + std::unique_ptr common_context( + new(std::nothrow) rmw_dds_common::Context()); + if (!common_context) { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + participant_info( + rmw_fastrtps_shared_cpp::create_participant( + eprosima_fastrtps_identifier, + context->options.domain_id, + &context->options.security_options, + (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, + common_context.get()), + [&](CustomParticipantInfo * participant_info) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { + fprintf( + stderr, "Failed to destroy participant after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!participant_info) { + return RMW_RET_BAD_ALLOC; + } + + rmw_qos_profile_t qos = rmw_qos_profile_default; + + qos.avoid_ros_namespace_conventions = false; // Change this to true after testing + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos.depth = 1; + qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + std::unique_ptr> + publisher( + rmw_fastrtps_dynamic_cpp::create_publisher( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "_participant_info", + &qos, + &publisher_options, + false, // our fastrtps typesupport doesn't support keyed topics + true), + [&](rmw_publisher_t * pub) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_publisher( + eprosima_fastrtps_identifier, + participant_info.get(), + pub)) + { + fprintf( + stderr, "Failed to destroy publisher after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!publisher) { + return RMW_RET_BAD_ALLOC; + } + + // If we would have support for keyed topics, this could be KEEP_LAST and depth 1. + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + std::unique_ptr> + subscription( + rmw_fastrtps_dynamic_cpp::create_subscription( + participant_info.get(), + rosidl_typesupport_cpp::get_message_type_support_handle(), + "_participant_info", + &qos, + &subscription_options, + false, // our fastrtps typesupport doesn't support keyed topics + true), + [&](rmw_subscription_t * sub) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_subscription( + eprosima_fastrtps_identifier, + participant_info.get(), + sub)) + { + fprintf( + stderr, "Failed to destroy subscription after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!subscription) { + return RMW_RET_BAD_ALLOC; + } + + common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, participant_info->participant->getGuid()); + common_context->pub = publisher.get(); + common_context->sub = subscription.get(); + + context->impl->common = common_context.get(); + context->impl->participant_info = participant_info.get(); + + if (RMW_RET_OK != rmw_fastrtps_dynamic_cpp::run_listener_thread(context)) { + return RMW_RET_ERROR; + } + common_context->graph_cache.add_participant(common_context->gid); + + publisher.release(); + subscription.release(); + common_context.release(); + participant_info.release(); + return RMW_RET_OK; +} + +rmw_ret_t +rmw_fastrtps_dynamic_cpp::register_node(rmw_context_t * context) +{ + assert(context); + assert(context->impl); + + std::lock_guard guard(context->impl->mutex); + + if (!context->impl->count) { + rmw_ret_t ret = init_context_impl(context); + if (RMW_RET_OK != ret) { + return ret; + } + } + context->impl->count++; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_fastrtps_dynamic_cpp::unregister_node(rmw_context_t * context) +{ + assert(context); + assert(context->impl); + + std::lock_guard guard(context->impl->mutex); + if (0u == --context->impl->count) { + rmw_ret_t ret; + ret = rmw_fastrtps_dynamic_cpp::join_listener_thread(context); + if (RMW_RET_OK != ret) { + return ret; + } + + auto common_context = static_cast(context->impl->common); + auto participant_info = static_cast(context->impl->participant_info); + + common_context->graph_cache.remove_participant(common_context->gid); + ret = rmw_fastrtps_shared_cpp::destroy_subscription( + eprosima_fastrtps_identifier, + participant_info, + common_context->sub); + if (RMW_RET_OK != ret) { + return ret; + } + ret = rmw_fastrtps_shared_cpp::destroy_publisher( + eprosima_fastrtps_identifier, + participant_info, + common_context->pub); + if (RMW_RET_OK != ret) { + return ret; + } + ret = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); + if (RMW_RET_OK != ret) { + return ret; + } + delete common_context; + context->impl->common = nullptr; + context->impl->participant_info = nullptr; + } + return RMW_RET_OK; +} diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 44823c8a8..3a815e027 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -29,6 +29,7 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" @@ -70,13 +71,15 @@ rmw_create_client( return nullptr; } - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + if (!participant_info) { + RMW_SET_ERROR_MSG("participant info is null"); return nullptr; } - Participant * participant = impl->participant; + Participant * participant = participant_info->participant; if (!participant) { RMW_SET_ERROR_MSG("participant handle is null"); return nullptr; @@ -160,7 +163,7 @@ rmw_create_client( _register_type(participant, info->response_type_support_); } - if (!impl->leave_middleware_default_qos) { + if (!participant_info->leave_middleware_default_qos) { subscriberParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } @@ -170,7 +173,7 @@ rmw_create_client( subscriberParam.topic.topicName = _create_topic_name( qos_policies, ros_service_response_prefix, service_name, "Reply"); - if (!impl->leave_middleware_default_qos) { + if (!participant_info->leave_middleware_default_qos) { publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; publisherParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; @@ -236,15 +239,54 @@ rmw_create_client( } memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_publisher_->getGuid()); + common_context->graph_cache.associate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( + gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + goto fail; + } + } + return rmw_client; fail: if (info != nullptr) { if (info->request_publisher_ != nullptr) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_publisher_->getGuid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removePublisher(info->request_publisher_); } if (info->response_subscriber_ != nullptr) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); + common_context->graph_cache.dissociate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removeSubscriber(info->response_subscriber_); } @@ -256,7 +298,7 @@ rmw_create_client( delete info->listener_; } - if (impl) { + if (participant_info) { if (info->request_type_support_ != nullptr) { rmw_fastrtps_shared_cpp::_unregister_type(participant, info->request_type_support_); } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp index 3e57dfbb8..7a979829a 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -12,10 +13,32 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + +#include "rcutils/types.h" + #include "rmw/impl/cpp/macros.hpp" +#include "rmw/init.h" +#include "rmw/init_options.h" +#include "rmw/publisher_options.h" #include "rmw/rmw.h" +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/participant.hpp" +#include "rmw_fastrtps_shared_cpp/publisher.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.hpp" + +#include "rosidl_typesupport_cpp/message_type_support.hpp" + #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/publisher.hpp" +#include "rmw_fastrtps_dynamic_cpp/subscription.hpp" + +#include "listener_thread.hpp" extern "C" { @@ -67,9 +90,19 @@ rmw_init_options_fini(rmw_init_options_t * init_options) return RMW_RET_OK; } +using rmw_dds_common::msg::ParticipantEntitiesInfo; + rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t * context) { + std::unique_ptr clean_when_fail( + context, + [](rmw_context_t * context) + { + *context = rmw_get_zero_initialized_context(); + }); + rmw_ret_t ret = RMW_RET_OK; + RCUTILS_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( @@ -79,7 +112,23 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); context->instance_id = options->instance_id; context->implementation_identifier = eprosima_fastrtps_identifier; - context->impl = nullptr; + + std::unique_ptr context_impl(new rmw_context_impl_t()); + if (!context_impl) { + return RMW_RET_BAD_ALLOC; + } + context->options = rmw_get_zero_initialized_init_options(); + ret = rmw_init_options_copy(options, &context->options); + if (RMW_RET_OK != ret) { + if (RMW_RET_OK != rmw_init_options_fini(&context->options)) { + fprintf( + stderr, + "Failed to destroy init options after ':" RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + return ret; + } + context->impl = context_impl.release(); + clean_when_fail.release(); return RMW_RET_OK; } @@ -106,8 +155,7 @@ rmw_context_fini(rmw_context_t * context) context->implementation_identifier, eprosima_fastrtps_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - // context impl is explicitly supposed to be nullptr for now, see rmw_init's code - // RCUTILS_CHECK_ARGUMENT_FOR_NULL(context->impl, RMW_RET_INVALID_ARGUMENT); + delete context->impl; *context = rmw_get_zero_initialized_context(); return RMW_RET_OK; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp index 09c6166e9..7fe7fb4f4 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp @@ -28,6 +28,7 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/register_node.hpp" extern "C" { @@ -37,9 +38,12 @@ rmw_create_node( const char * name, const char * namespace_, size_t domain_id, - const rmw_node_security_options_t * security_options, + const rmw_security_options_t * security_options, bool localhost_only) { + (void)domain_id; + (void)security_options; + (void)localhost_only; RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( init context, @@ -47,15 +51,24 @@ rmw_create_node( eprosima_fastrtps_identifier, // TODO(wjwwood): replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when refactored return NULL); + + if (RMW_RET_OK != rmw_fastrtps_dynamic_cpp::register_node(context)) { + return nullptr; + } + return rmw_fastrtps_shared_cpp::__rmw_create_node( - eprosima_fastrtps_identifier, name, namespace_, domain_id, security_options, localhost_only); + context, eprosima_fastrtps_identifier, name, namespace_); } rmw_ret_t rmw_destroy_node(rmw_node_t * node) { - return rmw_fastrtps_shared_cpp::__rmw_destroy_node( + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_node( eprosima_fastrtps_identifier, node); + if (RMW_RET_OK != ret) { + return ret; + } + return rmw_fastrtps_dynamic_cpp::unregister_node(node->context); } rmw_ret_t diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp index 3ee5c34f2..6dbd0786c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -20,23 +20,18 @@ #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" -#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" -#include "rmw_fastrtps_shared_cpp/names.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" -#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/publisher.hpp" + +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" #include "type_support_common.hpp" #include "type_support_registry.hpp" -using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; -using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; - extern "C" { rmw_ret_t @@ -80,172 +75,42 @@ rmw_create_publisher( return nullptr; } - if (!topic_name || strlen(topic_name) == 0) { - RMW_SET_ERROR_MSG("publisher topic is null or empty string"); - return nullptr; - } - - if (!qos_policies) { - RMW_SET_ERROR_MSG("qos_policies is null"); - return nullptr; - } - - if (!publisher_options) { - RMW_SET_ERROR_MSG("publisher_options is null"); - return nullptr; - } - - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return nullptr; - } - - Participant * participant = impl->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } + auto impl = static_cast(node->context->impl->participant_info); - const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); - if (!type_support) { - type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (!type_support) { - RMW_SET_ERROR_MSG("type support not from this implementation"); - return nullptr; - } - } + rmw_publisher_t * publisher = rmw_fastrtps_dynamic_cpp::create_publisher( + impl, + type_supports, + topic_name, + qos_policies, + publisher_options, + false, + true); - if (!is_valid_qos(*qos_policies)) { + if (!publisher) { return nullptr; } - CustomPublisherInfo * info = nullptr; - rmw_publisher_t * rmw_publisher = nullptr; - eprosima::fastrtps::PublisherAttributes publisherParam; - - // Load default XML profile. - Domain::getDefaultPublisherAttributes(publisherParam); + auto common_context = static_cast(node->context->impl->common); - // TODO(karsten1987) Verify consequences for std::unique_ptr? - info = new (std::nothrow) CustomPublisherInfo(); - if (!info) { - RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); - return nullptr; - } - - TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); - auto type_impl = type_registry.get_message_type_support(type_support); - if (!type_impl) { - delete info; - RMW_SET_ERROR_MSG("failed to allocate type support"); - return nullptr; - } - - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_impl; - - std::string type_name = _create_type_name( - type_support->data, info->typesupport_identifier_); - if (!Domain::getRegisteredType( - participant, type_name.c_str(), - reinterpret_cast(&info->type_support_))) + auto info = static_cast(publisher->data); { - info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); - goto fail; - } - _register_type(participant, info->type_support_); - } - - if (!impl->leave_middleware_default_qos) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - } - - publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - publisherParam.topic.topicDataType = type_name; - publisherParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); - - // 1 Heartbeat every 10ms - // publisherParam.times.heartbeatPeriod.seconds = 0; - // publisherParam.times.heartbeatPeriod.fraction = 42949673; - - // 300000 bytes each 10ms - // ThroughputControllerDescriptor throughputController{3000000, 10}; - // publisherParam.throughputController = throughputController; - - if (!get_datawriter_qos(*qos_policies, publisherParam)) { - RMW_SET_ERROR_MSG("failed to get datawriter qos"); - goto fail; - } - - info->listener_ = new (std::nothrow) PubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); - goto fail; - } - - info->publisher_ = Domain::createPublisher(participant, publisherParam, info->listener_); - if (!info->publisher_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); - goto fail; - } - - info->publisher_gid.implementation_identifier = eprosima_fastrtps_identifier; - static_assert( - sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, - "RMW_GID_STORAGE_SIZE insufficient to store the rmw_fastrtps_dynamic_cpp GID implementation." - ); - - memset(info->publisher_gid.data, 0, RMW_GID_STORAGE_SIZE); - rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( - info->publisher_->getGuid(), - info->publisher_gid.data); - - rmw_publisher = rmw_publisher_allocate(); - if (!rmw_publisher) { - RMW_SET_ERROR_MSG("failed to allocate publisher"); - goto fail; - } - rmw_publisher->can_loan_messages = false; - rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; - rmw_publisher->data = info; - rmw_publisher->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); - - if (!rmw_publisher->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); - goto fail; - } - - memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); - - rmw_publisher->options = *publisher_options; - - return rmw_publisher; - -fail: - if (info) { - if (info->type_support_ != nullptr) { - delete info->type_support_; - } - if (info->listener_ != nullptr) { - delete info->listener_; + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_writer( + info->publisher_gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + rmw_fastrtps_shared_cpp::__rmw_destroy_publisher( + eprosima_fastrtps_identifier, node, publisher); + return nullptr; } - delete info; - } - - type_registry.return_message_type_support(type_support); - - if (rmw_publisher) { - rmw_publisher_free(rmw_publisher); } - - return nullptr; + return publisher; } rmw_ret_t @@ -300,6 +165,8 @@ rmw_return_loaned_message_from_publisher( return RMW_RET_UNSUPPORTED; } +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; + rmw_ret_t rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_security.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_security.cpp new file mode 100644 index 000000000..6774438dd --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_security.cpp @@ -0,0 +1,24 @@ +// 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 "rmw/security.h" + +extern "C" +{ +bool +rmw_use_node_name_in_security_directory_lookup() +{ + return false; +} +} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 12975bf4c..8f8b5648e 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -35,6 +35,7 @@ #include "rosidl_typesupport_introspection_cpp/identifier.hpp" #include "rosidl_typesupport_introspection_c/identifier.h" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" #include "rmw_fastrtps_shared_cpp/names.hpp" @@ -81,7 +82,9 @@ rmw_create_service( return nullptr; } - const CustomParticipantInfo * impl = static_cast(node->data); + const CustomParticipantInfo * impl = + static_cast(node->context->impl->participant_info); + auto common_context = static_cast(node->context->impl->common); if (!impl) { RMW_SET_ERROR_MSG("node impl is null"); return nullptr; @@ -241,16 +244,55 @@ rmw_create_service( } memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_subscriber_->getGuid()); + common_context->graph_cache.associate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_writer( + gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + goto fail; + } + } + return rmw_service; fail: if (info) { if (info->response_publisher_) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removePublisher(info->response_publisher_); } if (info->request_subscriber_) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_subscriber_->getGuid()); + common_context->graph_cache.dissociate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removeSubscriber(info->request_subscriber_); } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp index 594f0c017..67d0583a0 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include #include "rmw/allocators.h" #include "rmw/error_handling.h" @@ -21,25 +21,15 @@ #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" -#include "rmw_fastrtps_shared_cpp/names.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" -#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" - -#include "fastrtps/participant/Participant.h" -#include "fastrtps/subscriber/Subscriber.h" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/subscription.hpp" #include "type_support_common.hpp" #include "type_support_registry.hpp" -using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; -using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; - extern "C" { rmw_ret_t @@ -83,152 +73,41 @@ rmw_create_subscription( return nullptr; } - if (!topic_name || strlen(topic_name) == 0) { - RMW_SET_ERROR_MSG("subscription topic is null or empty string"); - return nullptr; - } - - if (!qos_policies) { - RMW_SET_ERROR_MSG("qos_policies is null"); - return nullptr; - } - - if (!subscription_options) { - RMW_SET_ERROR_MSG("subscription_options is null"); - return nullptr; - } - - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return nullptr; - } - - Participant * participant = impl->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } - - const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); - if (!type_support) { - type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (!type_support) { - RMW_SET_ERROR_MSG("type support not from this implementation"); - return nullptr; - } - } - - if (!is_valid_qos(*qos_policies)) { - return nullptr; - } - - CustomSubscriberInfo * info = nullptr; - rmw_subscription_t * rmw_subscription = nullptr; - eprosima::fastrtps::SubscriberAttributes subscriberParam; - - // Load default XML profile. - Domain::getDefaultSubscriberAttributes(subscriberParam); - - info = new (std::nothrow) CustomSubscriberInfo(); - if (!info) { - RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); - return nullptr; - } + auto participant_info = + static_cast(node->context->impl->participant_info); - TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); - auto type_impl = type_registry.get_message_type_support(type_support); - if (!type_impl) { - delete info; - RMW_SET_ERROR_MSG("failed to allocate type support"); + rmw_subscription_t * subscription = rmw_fastrtps_dynamic_cpp::create_subscription( + participant_info, + type_supports, + topic_name, + qos_policies, + subscription_options, + false, + true); + if (!subscription) { return nullptr; } - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_impl; - - std::string type_name = _create_type_name( - type_support->data, info->typesupport_identifier_); - if (!Domain::getRegisteredType( - participant, type_name.c_str(), - reinterpret_cast(&info->type_support_))) + auto common_context = static_cast(node->context->impl->common); + auto info = static_cast(subscription->data); { - info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); - goto fail; - } - _register_type(participant, info->type_support_); - } - - if (!impl->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - } - - subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - subscriberParam.topic.topicDataType = type_name; - subscriberParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); - - if (!get_datareader_qos(*qos_policies, subscriberParam)) { - RMW_SET_ERROR_MSG("failed to get datareader qos"); - goto fail; - } - - info->listener_ = new (std::nothrow) SubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); - goto fail; - } - - info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); - if (!info->subscriber_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); - goto fail; - } - - rmw_subscription = rmw_subscription_allocate(); - if (!rmw_subscription) { - RMW_SET_ERROR_MSG("failed to allocate subscription"); - goto fail; - } - rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; - rmw_subscription->data = info; - rmw_subscription->topic_name = - reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); - - if (!rmw_subscription->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); - goto fail; - } - - memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); - - rmw_subscription->options = *subscription_options; - rmw_subscription->can_loan_messages = false; - return rmw_subscription; - -fail: - - if (info != nullptr) { - if (info->type_support_ != nullptr) { - delete info->type_support_; - } - if (info->listener_ != nullptr) { - delete info->listener_; + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( + info->subscription_gid_, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( + eprosima_fastrtps_identifier, node, subscription); + return nullptr; } - delete info; - } - - type_registry.return_message_type_support(type_support); - - if (rmw_subscription) { - rmw_subscription_free(rmw_subscription); } - - return nullptr; + return subscription; } rmw_ret_t @@ -249,6 +128,8 @@ rmw_subscription_get_actual_qos( subscription, qos); } +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; + rmw_ret_t rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) { diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp new file mode 100644 index 000000000..5ea84f5a3 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -0,0 +1,194 @@ +// 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 "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "fastrtps/participant/Participant.h" +#include "fastrtps/subscriber/Subscriber.h" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/subscription.hpp" + +#include "type_support_common.hpp" +#include "type_support_registry.hpp" + +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; +using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; + + +namespace rmw_fastrtps_dynamic_cpp +{ + +rmw_subscription_t * +create_subscription( + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_subscription_options_t * subscription_options, + bool keyed, + bool create_subscription_listener) +{ + (void)keyed; + (void)create_subscription_listener; + + if (!topic_name || strlen(topic_name) == 0) { + RMW_SET_ERROR_MSG("subscription topic is null or empty string"); + return nullptr; + } + + if (!qos_policies) { + RMW_SET_ERROR_MSG("qos_policies is null"); + return nullptr; + } + + if (!subscription_options) { + RMW_SET_ERROR_MSG("subscription_options is null"); + return nullptr; + } + + Participant * participant = participant_info->participant; + if (!participant) { + RMW_SET_ERROR_MSG("participant handle is null"); + return nullptr; + } + + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return nullptr; + } + } + + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + + CustomSubscriberInfo * info = nullptr; + rmw_subscription_t * rmw_subscription = nullptr; + eprosima::fastrtps::SubscriberAttributes subscriberParam; + + // Load default XML profile. + Domain::getDefaultSubscriberAttributes(subscriberParam); + + info = new (std::nothrow) CustomSubscriberInfo(); + if (!info) { + RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); + return nullptr; + } + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto type_impl = type_registry.get_message_type_support(type_support); + if (!type_impl) { + delete info; + RMW_SET_ERROR_MSG("failed to allocate type support"); + return nullptr; + } + + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_impl; + + std::string type_name = _create_type_name( + type_support->data, info->typesupport_identifier_); + if ( + !Domain::getRegisteredType( + participant, type_name.c_str(), + reinterpret_cast(&info->type_support_))) + { + info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); + if (!info->type_support_) { + RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); + goto fail; + } + _register_type(participant, info->type_support_); + } + + if (!participant_info->leave_middleware_default_qos) { + subscriberParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; + subscriberParam.topic.topicDataType = type_name; + subscriberParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); + + if (!get_datareader_qos(*qos_policies, subscriberParam)) { + RMW_SET_ERROR_MSG("failed to get datareader qos"); + goto fail; + } + + info->listener_ = new (std::nothrow) SubListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); + goto fail; + } + + info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); + if (!info->subscriber_) { + RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); + goto fail; + } + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->subscriber_->getGuid()); + rmw_subscription = rmw_subscription_allocate(); + if (!rmw_subscription) { + RMW_SET_ERROR_MSG("failed to allocate subscription"); + goto fail; + } + rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; + rmw_subscription->data = info; + rmw_subscription->topic_name = + reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + + if (!rmw_subscription->topic_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); + goto fail; + } + + memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); + + rmw_subscription->options = *subscription_options; + rmw_subscription->can_loan_messages = false; + return rmw_subscription; + +fail: + if (info != nullptr) { + delete info->type_support_; + delete info->listener_; + delete info; + } + type_registry.return_message_type_support(type_support); + rmw_subscription_free(rmw_subscription); + return nullptr; +} +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index 32abbcb64..5e430e367 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -37,6 +37,7 @@ find_package(ament_cmake_ros REQUIRED) find_package(rcpputils REQUIRED) find_package(rcutils REQUIRED) +find_package(rmw_dds_common REQUIRED) find_package(fastrtps_cmake_module REQUIRED) find_package(fastcdr REQUIRED CONFIG) @@ -49,8 +50,11 @@ include_directories(include) add_library(rmw_fastrtps_shared_cpp src/custom_publisher_info.cpp src/custom_subscriber_info.cpp + src/create_rmw_gid.cpp src/demangle.cpp src/namespace_prefix.cpp + src/participant.cpp + src/publisher.cpp src/qos.cpp src/rmw_client.cpp src/rmw_compare_gids_equal.cpp @@ -76,6 +80,7 @@ add_library(rmw_fastrtps_shared_cpp src/rmw_trigger_guard_condition.cpp src/rmw_wait.cpp src/rmw_wait_set.cpp + src/subscription.cpp src/TypeSupport_impl.cpp ) @@ -88,6 +93,7 @@ ament_target_dependencies(rmw_fastrtps_shared_cpp "rcpputils" "rcutils" "rmw" + "rmw_dds_common" ) # Causes the visibility macros to use dllexport rather than dllimport, diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/create_rmw_gid.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/create_rmw_gid.hpp new file mode 100644 index 000000000..0435bc4ab --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/create_rmw_gid.hpp @@ -0,0 +1,33 @@ +// 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_FASTRTPS_SHARED_CPP__CREATE_RMW_GID_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__CREATE_RMW_GID_HPP_ + +#include "fastrtps/rtps/common/Guid.h" + +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_gid_t +create_rmw_gid(const char * identifier, const eprosima::fastrtps::rtps::GUID_t & guid); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__CREATE_RMW_GID_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 3ce19dbd3..a63617dd8 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -20,6 +20,7 @@ #include #include +#include "fastrtps/rtps/common/InstanceHandle.h" #include "fastrtps/attributes/ParticipantAttributes.h" #include "fastrtps/participant/Participant.h" #include "fastrtps/participant/ParticipantListener.h" @@ -27,12 +28,16 @@ #include "rcpputils/thread_safety_annotations.hpp" #include "rcutils/logging_macros.h" -#include "rmw/impl/cpp/key_value.hpp" +#include "rmw/qos_profiles.h" #include "rmw/rmw.h" -#include "rmw_common.hpp" +#include "rmw_dds_common/context.hpp" -#include "topic_cache.hpp" +#include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +using rmw_dds_common::operator<<; class ParticipantListener; @@ -53,88 +58,28 @@ typedef struct CustomParticipantInfo class ParticipantListener : public eprosima::fastrtps::ParticipantListener { public: - explicit ParticipantListener(rmw_guard_condition_t * graph_guard_condition) - : graph_guard_condition_(graph_guard_condition) + explicit ParticipantListener( + rmw_guard_condition_t * graph_guard_condition, + rmw_dds_common::Context * context) + : context(context), + graph_guard_condition_(graph_guard_condition) {} void onParticipantDiscovery( eprosima::fastrtps::Participant *, eprosima::fastrtps::rtps::ParticipantDiscoveryInfo && info) override { + // We aren't monitoring discovered participants, just dropped and removed. + // Participants are added to the Graph when they send a ParticipantEntitiesInfo message. if ( - info.status != eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DISCOVERED_PARTICIPANT && info.status != eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT && info.status != eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT) { return; } - - std::lock_guard guard(names_mutex_); - if (eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DISCOVERED_PARTICIPANT == info.status) { - // ignore already known GUIDs - if (discovered_names.find(info.info.m_guid) == discovered_names.end()) { - auto map = rmw::impl::cpp::parse_key_value(info.info.m_userData); - auto name_found = map.find("name"); - auto ns_found = map.find("namespace"); - - std::string name; - if (name_found != map.end()) { - name = std::string(name_found->second.begin(), name_found->second.end()); - } - - std::string namespace_; - if (ns_found != map.end()) { - namespace_ = std::string(ns_found->second.begin(), ns_found->second.end()); - } - - if (name.empty()) { - // use participant name if no name was found in the user data - name = info.info.m_participantName; - } - // ignore discovered participants without a name - if (!name.empty()) { - discovered_names[info.info.m_guid] = name; - discovered_namespaces[info.info.m_guid] = namespace_; - } - } - } else { - { - auto it = discovered_names.find(info.info.m_guid); - // only consider known GUIDs - if (it != discovered_names.end()) { - discovered_names.erase(it); - } - } - { - auto it = discovered_namespaces.find(info.info.m_guid); - // only consider known GUIDs - if (it != discovered_namespaces.end()) { - discovered_namespaces.erase(it); - } - } - } - } - - std::vector get_discovered_names() const - { - std::lock_guard guard(names_mutex_); - std::vector names(discovered_names.size()); - size_t i = 0; - for (auto it : discovered_names) { - names[i++] = it.second; - } - return names; - } - - std::vector get_discovered_namespaces() const - { - std::lock_guard guard(names_mutex_); - std::vector namespaces(discovered_namespaces.size()); - size_t i = 0; - for (auto it : discovered_namespaces) { - namespaces[i++] = it.second; - } - return namespaces; + context->graph_cache.remove_participant( + rmw_fastrtps_shared_cpp::create_rmw_gid( + graph_guard_condition_->implementation_identifier, info.info.m_guid)); } void onSubscriberDiscovery( @@ -159,27 +104,34 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener } } +private: template - void process_discovery_info(T & proxyData, bool is_alive, bool is_reader) + void + process_discovery_info(T & proxyData, bool is_alive, bool is_reader) { - auto & topic_cache = - is_reader ? reader_topic_cache : writer_topic_cache; bool trigger; { - std::lock_guard guard(topic_cache.getMutex()); if (is_alive) { - trigger = topic_cache().addTopic( - proxyData.RTPSParticipantKey(), - proxyData.guid(), + rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown; + dds_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); + + trigger = context->graph_cache.add_entity( + rmw_fastrtps_shared_cpp::create_rmw_gid( + graph_guard_condition_->implementation_identifier, + proxyData.guid()), proxyData.topicName().to_string(), proxyData.typeName().to_string(), - proxyData.m_qos); + rmw_fastrtps_shared_cpp::create_rmw_gid( + graph_guard_condition_->implementation_identifier, + iHandle2GUID(proxyData.RTPSParticipantKey())), + qos_profile, + is_reader); } else { - trigger = topic_cache().removeTopic( - proxyData.RTPSParticipantKey(), - proxyData.guid(), - proxyData.topicName().to_string(), - proxyData.typeName().to_string()); + trigger = context->graph_cache.remove_entity( + rmw_fastrtps_shared_cpp::create_rmw_gid( + graph_guard_condition_->implementation_identifier, + proxyData.guid()), + is_reader); } } if (trigger) { @@ -189,12 +141,7 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener } } - using guid_map_t = std::map; - mutable std::mutex names_mutex_; - guid_map_t discovered_names RCPPUTILS_TSA_GUARDED_BY(names_mutex_); - guid_map_t discovered_namespaces RCPPUTILS_TSA_GUARDED_BY(names_mutex_); - LockedObject reader_topic_cache; - LockedObject writer_topic_cache; + rmw_dds_common::Context * context; rmw_guard_condition_t * graph_guard_condition_; }; diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp index 7d213f742..e46bf9e81 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -34,7 +34,7 @@ class SubListener; -typedef struct CustomSubscriberInfo : public CustomEventInfo +struct CustomSubscriberInfo : public CustomEventInfo { virtual ~CustomSubscriberInfo() = default; @@ -42,12 +42,13 @@ typedef struct CustomSubscriberInfo : public CustomEventInfo SubListener * listener_; rmw_fastrtps_shared_cpp::TypeSupport * type_support_; const void * type_support_impl_; + rmw_gid_t subscription_gid_; const char * typesupport_identifier_; RMW_FASTRTPS_SHARED_CPP_PUBLIC EventListenerInterface * getListener() const final; -} CustomSubscriberInfo; +}; class SubListener : public EventListenerInterface, public eprosima::fastrtps::SubscriberListener { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp index a03c71aa8..423286d19 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp @@ -15,27 +15,27 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__NAMES_HPP_ #define RMW_FASTRTPS_SHARED_CPP__NAMES_HPP_ +#include + #include "fastrtps/utils/fixed_size_string.hpp" #include "rmw/types.h" #include "namespace_prefix.hpp" -/// Construct a topic name according to proper conventions. +/// Construct a topic name. /** - * \param[in] qos_profile The QoS profile for the topic. * \param[in] prefix Required prefix for topic name. * \param[in] base Required name of the topic. * \param[in] suffix Optional suffix for topic name. */ inline -eprosima::fastrtps::string_255 -_create_topic_name( - const rmw_qos_profile_t * qos_profile, +std::string +_mangle_topic_name( const char * prefix, const char * base, const char * suffix = nullptr) { std::ostringstream topicName; - if (!qos_profile->avoid_ros_namespace_conventions && prefix) { + if (prefix) { topicName << prefix; } topicName << base; @@ -45,4 +45,25 @@ _create_topic_name( return topicName.str(); } +/// Construct a topic name according to proper conventions. +/** + * \param[in] qos_profile The QoS profile for the topic. + * \param[in] prefix Required prefix for topic name. + * \param[in] base Required name of the topic. + * \param[in] suffix Optional suffix for topic name. + */ +inline +std::string +_create_topic_name( + const rmw_qos_profile_t * qos_profile, + const char * prefix, + const char * base, + const char * suffix = nullptr) +{ + if (qos_profile->avoid_ros_namespace_conventions) { + prefix = nullptr; + } + return _mangle_topic_name(prefix, base, suffix); +} + #endif // RMW_FASTRTPS_SHARED_CPP__NAMES_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/namespace_prefix.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/namespace_prefix.hpp index fa4512497..08bb3a787 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/namespace_prefix.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/namespace_prefix.hpp @@ -29,6 +29,16 @@ RMW_FASTRTPS_SHARED_CPP_PUBLIC extern const char * const ros_service_response_pr RMW_FASTRTPS_SHARED_CPP_PUBLIC extern const std::vector _ros_prefixes; } // extern "C" +/// Returns `name` stripped of `prefix` if exists, if not return "". +/** + * \param[in] name string that will be stripped from prefix + * \param[in] prefix prefix to be stripped + * \return name stripped of prefix, or + * \return "" if name doesn't start with prefix + */ +std::string +_resolve_prefix(const std::string & name, const std::string & prefix); + /// Return the ROS specific prefix if it exists, otherwise "". std::string _get_ros_prefix_if_exists(const std::string & topic_name); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp new file mode 100644 index 000000000..b5608f632 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp @@ -0,0 +1,39 @@ +// 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_FASTRTPS_SHARED_CPP__PARTICIPANT_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__PARTICIPANT_HPP_ + +#include "rmw/types.h" + +#include "rmw_dds_common/context.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +CustomParticipantInfo * +create_participant( + const char * identifier, + size_t domain_id, + const rmw_security_options_t * security_options, + bool localhost_only, + rmw_dds_common::Context * common_context); + +rmw_ret_t +destroy_participant(CustomParticipantInfo * info); +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__PARTICIPANT_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/publisher.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/publisher.hpp new file mode 100644 index 000000000..3dae78b6c --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/publisher.hpp @@ -0,0 +1,32 @@ +// 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_FASTRTPS_SHARED_CPP__PUBLISHER_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__PUBLISHER_HPP_ + +#include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +rmw_ret_t +destroy_publisher( + const char * identifier, + CustomParticipantInfo * participant_info, + rmw_publisher_t * publisher); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__PUBLISHER_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index fed276204..1857abba7 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -15,11 +16,15 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ #define RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ +#include "fastrtps/attributes/PublisherAttributes.h" +#include "fastrtps/attributes/SubscriberAttributes.h" +#include "fastrtps/qos/QosPolicies.h" +#include "fastrtps/qos/ReaderQos.h" +#include "fastrtps/qos/WriterQos.h" + #include "rmw/rmw.h" -#include "./visibility_control.h" -#include "fastrtps/qos/WriterQos.h" -#include "fastrtps/qos/ReaderQos.h" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" namespace eprosima { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp index 13da08486..1ef7e2ac3 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -86,12 +86,10 @@ __rmw_set_log_severity(rmw_log_severity_t severity); RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_node_t * __rmw_create_node( + rmw_context_t * context, const char * identifier, const char * name, - const char * namespace_, - size_t domain_id, - const rmw_node_security_options_t * security_options, - bool localhost_only); + const char * namespace_); RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t @@ -152,7 +150,7 @@ RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t __rmw_destroy_publisher( const char * identifier, - rmw_node_t * node, + const rmw_node_t * node, rmw_publisher_t * publisher); RMW_FASTRTPS_SHARED_CPP_PUBLIC @@ -270,7 +268,7 @@ RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t __rmw_destroy_subscription( const char * identifier, - rmw_node_t * node, + const rmw_node_t * node, rmw_subscription_t * subscription); RMW_FASTRTPS_SHARED_CPP_PUBLIC diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_context_impl.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_context_impl.hpp new file mode 100644 index 000000000..6a985561a --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_context_impl.hpp @@ -0,0 +1,28 @@ +// 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 RMW_FASTRTPS_SHARED_CPP__RMW_CONTEXT_IMPL_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__RMW_CONTEXT_IMPL_HPP_ + +#include + +struct rmw_context_impl_t +{ + void * common; + void * participant_info; + std::mutex mutex; + uint64_t count; +}; + +#endif // RMW_FASTRTPS_SHARED_CPP__RMW_CONTEXT_IMPL_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/subscription.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/subscription.hpp new file mode 100644 index 000000000..b959a3669 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/subscription.hpp @@ -0,0 +1,32 @@ +// 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_FASTRTPS_SHARED_CPP__SUBSCRIPTION_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__SUBSCRIPTION_HPP_ + +#include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +rmw_ret_t +destroy_subscription( + const char * identifier, + CustomParticipantInfo * participant_info, + rmw_subscription_t * subscription); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__SUBSCRIPTION_HPP_ diff --git a/rmw_fastrtps_shared_cpp/package.xml b/rmw_fastrtps_shared_cpp/package.xml index e0a0b8931..837d1f117 100644 --- a/rmw_fastrtps_shared_cpp/package.xml +++ b/rmw_fastrtps_shared_cpp/package.xml @@ -19,6 +19,7 @@ rcpputils rcutils rmw + rmw_dds_common fastcdr fastrtps @@ -26,6 +27,7 @@ rcpputils rcutils rmw + rmw_dds_common ament_lint_auto ament_lint_common diff --git a/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp b/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp new file mode 100644 index 000000000..b7de21922 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp @@ -0,0 +1,36 @@ +// 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 "fastrtps/rtps/common/Guid.h" + +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp" +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" + +rmw_gid_t +rmw_fastrtps_shared_cpp::create_rmw_gid( + const char * identifier, const eprosima::fastrtps::rtps::GUID_t & guid) +{ + rmw_gid_t rmw_gid = {}; + rmw_gid.implementation_identifier = identifier; + static_assert( + sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, + "RMW_GID_STORAGE_SIZE insufficient to store the fastrtps GUID_t." + ); + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + info->publisher_->getGuid(), + info->publisher_gid.data); + return rmw_gid; +} diff --git a/rmw_fastrtps_shared_cpp/src/demangle.cpp b/rmw_fastrtps_shared_cpp/src/demangle.cpp index 752355888..fd9103f70 100644 --- a/rmw_fastrtps_shared_cpp/src/demangle.cpp +++ b/rmw_fastrtps_shared_cpp/src/demangle.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -22,6 +23,8 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "demangle.hpp" + /// Return the demangle ROS topic or the original if not a ROS topic. std::string _demangle_if_ros_topic(const std::string & topic_name) @@ -52,49 +55,34 @@ _demangle_if_ros_type(const std::string & dds_type_string) return type_namespace + type_name; } +/// Return the topic name for a given topic if it is part of one, else "". +std::string +_demangle_ros_topic_from_topic(const std::string & topic_name) +{ + return _resolve_prefix(topic_name, ros_topic_prefix); +} + /// Return the service name for a given topic if it is part of one, else "". std::string -_demangle_service_from_topic(const std::string & topic_name) +_demangle_service_from_topic( + const std::string & prefix, const std::string & topic_name, std::string suffix) { - std::string prefix = _get_ros_prefix_if_exists(topic_name); - if (prefix.empty()) { - // not a ROS topic or service + std::string service_name = _resolve_prefix(topic_name, prefix); + if ("" == service_name) { return ""; } - std::vector prefixes = { - ros_service_response_prefix, - ros_service_requester_prefix, - }; - if ( - std::none_of( - prefixes.cbegin(), prefixes.cend(), - [&prefix](auto x) { - return prefix == x; - })) - { - // not a ROS service topic - return ""; - } - std::vector suffixes = { - "Reply", - "Request", - }; - std::string found_suffix; - size_t suffix_position = std::string::npos; - for (auto suffix : suffixes) { - suffix_position = topic_name.rfind(suffix); - if (suffix_position != std::string::npos) { - if (topic_name.length() - suffix_position - suffix.length() != 0) { - RCUTILS_LOG_WARN_NAMED( - "rmw_fastrtps_shared_cpp", - "service topic has service prefix and a suffix, but not at the end" - ", report this: '%s'", topic_name.c_str()); - continue; - } - found_suffix = suffix; - break; + + size_t suffix_position = service_name.rfind(suffix); + if (suffix_position != std::string::npos) { + if (service_name.length() - suffix_position - suffix.length() != 0) { + RCUTILS_LOG_WARN_NAMED( + "rmw_fastrtps_shared_cpp", + "service topic has service prefix and a suffix, but not at the end" + ", report this: '%s'", topic_name.c_str()); + return ""; } } + if (std::string::npos == suffix_position) { RCUTILS_LOG_WARN_NAMED( "rmw_fastrtps_shared_cpp", @@ -102,11 +90,30 @@ _demangle_service_from_topic(const std::string & topic_name) ", report this: '%s'", topic_name.c_str()); return ""; } - // strip off the suffix first - std::string service_name = topic_name.substr(0, suffix_position + 1); - // then the prefix - size_t start = prefix.length(); // explicitly leave / after prefix - return service_name.substr(start, service_name.length() - 1 - start); + return service_name.substr(0, suffix_position); +} + +std::string +_demangle_service_from_topic(const std::string & topic_name) +{ + const std::string demangled_topic = _demangle_service_reply_from_topic(topic_name); + if ("" != demangled_topic) { + return demangled_topic; + } + return _demangle_service_request_from_topic(topic_name); +} + + +std::string +_demangle_service_request_from_topic(const std::string & topic_name) +{ + return _demangle_service_from_topic(ros_service_requester_prefix, topic_name, "Request"); +} + +std::string +_demangle_service_reply_from_topic(const std::string & topic_name) +{ + return _demangle_service_from_topic(ros_service_response_prefix, topic_name, "Reply"); } /// Return the demangled service type if it is a ROS srv type, else "". @@ -154,3 +161,9 @@ _demangle_service_type_only(const std::string & dds_type_name) std::string type_name = dds_type_name.substr(start, suffix_position - start); return type_namespace + type_name; } + +std::string +_identity_demangle(const std::string & name) +{ + return name; +} diff --git a/rmw_fastrtps_shared_cpp/src/demangle.hpp b/rmw_fastrtps_shared_cpp/src/demangle.hpp index 65efe9918..02a3e005a 100644 --- a/rmw_fastrtps_shared_cpp/src/demangle.hpp +++ b/rmw_fastrtps_shared_cpp/src/demangle.hpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -25,12 +26,32 @@ _demangle_if_ros_topic(const std::string & topic_name); std::string _demangle_if_ros_type(const std::string & dds_type_string); -/// Return the service name for a given topic if it is part of one, else "". +/// Return the topic name for a given topic if it is part of one, else "". +std::string +_demangle_ros_topic_from_topic(const std::string & topic_name); + +/// Return the service name for a given topic if it is part of a service, else "". std::string _demangle_service_from_topic(const std::string & topic_name); +/// Return the service name for a given topic if it is part of a service request, else "". +std::string +_demangle_service_request_from_topic(const std::string & topic_name); + +/// Return the service name for a given topic if it is part of a service reply, else "". +std::string +_demangle_service_reply_from_topic(const std::string & topic_name); + /// Return the demangled service type if it is a ROS srv type, else "". std::string _demangle_service_type_only(const std::string & dds_type_name); +/// Used when ros names are not mangled. +std::string +_identity_demangle(const std::string & name); + + +using DemangleFunction = std::string (*)(const std::string &); +using MangleFunction = DemangleFunction; + #endif // DEMANGLE_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/namespace_prefix.cpp b/rmw_fastrtps_shared_cpp/src/namespace_prefix.cpp index 5d58079d9..f42b689e1 100644 --- a/rmw_fastrtps_shared_cpp/src/namespace_prefix.cpp +++ b/rmw_fastrtps_shared_cpp/src/namespace_prefix.cpp @@ -27,6 +27,16 @@ const std::vector _ros_prefixes = {ros_topic_prefix, ros_service_requester_prefix, ros_service_response_prefix}; } // extern "C" +/// Returns `name` stripped of `prefix`. +std::string +_resolve_prefix(const std::string & name, const std::string & prefix) +{ + if (name.rfind(prefix, 0) == 0 && name.at(prefix.length()) == '/') { + return name.substr(prefix.length()); + } + return ""; +} + /// Return the ROS specific prefix if it exists, otherwise "". std::string _get_ros_prefix_if_exists(const std::string & topic_name) diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp new file mode 100644 index 000000000..12ede8dae --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -0,0 +1,242 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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 "fastrtps/config.h" +#include "fastrtps/Domain.h" +#include "fastrtps/attributes/ParticipantAttributes.h" +#include "fastrtps/attributes/PublisherAttributes.h" +#include "fastrtps/attributes/SubscriberAttributes.h" +#include "fastrtps/participant/Participant.h" +#include "fastrtps/publisher/Publisher.h" +#include "fastrtps/publisher/PublisherListener.h" +#include "fastrtps/rtps/RTPSDomain.h" +#include "fastrtps/rtps/common/Locator.h" +#include "fastrtps/rtps/builtin/discovery/endpoint/EDPSimple.h" +#include "fastrtps/rtps/reader/ReaderListener.h" +#include "fastrtps/rtps/reader/RTPSReader.h" +#include "fastrtps/rtps/reader/StatefulReader.h" +#include "fastrtps/subscriber/Subscriber.h" +#include "fastrtps/subscriber/SubscriberListener.h" +#include "fastrtps/subscriber/SampleInfo.h" + +#include "rcutils/get_env.h" + +#include "rmw/allocators.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/participant.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +using Domain = eprosima::fastrtps::Domain; +using IPLocator = eprosima::fastrtps::rtps::IPLocator; +using Locator_t = eprosima::fastrtps::rtps::Locator_t; +using Participant = eprosima::fastrtps::Participant; +using ParticipantAttributes = eprosima::fastrtps::ParticipantAttributes; +using StatefulReader = eprosima::fastrtps::rtps::StatefulReader; + +static +CustomParticipantInfo * +__create_participant( + const char * identifier, + ParticipantAttributes participantAttrs, + bool leave_middleware_default_qos, + rmw_dds_common::Context * common_context) +{ + // Declare everything before beginning to create things. + ::ParticipantListener * listener = nullptr; + Participant * participant = nullptr; + rmw_guard_condition_t * graph_guard_condition = nullptr; + CustomParticipantInfo * participant_info = nullptr; + + graph_guard_condition = rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(identifier); + if (!graph_guard_condition) { + // error already set + goto fail; + } + + try { + listener = new ::ParticipantListener( + graph_guard_condition, common_context); + } catch (std::bad_alloc &) { + RMW_SET_ERROR_MSG("failed to allocate participant listener"); + goto fail; + } + + participant = Domain::createParticipant(participantAttrs, listener); + if (!participant) { + RMW_SET_ERROR_MSG("create_node() could not create participant"); + return nullptr; + } + + try { + participant_info = new CustomParticipantInfo(); + } catch (std::bad_alloc &) { + RMW_SET_ERROR_MSG("failed to allocate node impl struct"); + goto fail; + } + participant_info->leave_middleware_default_qos = leave_middleware_default_qos; + + participant_info->participant = participant; + participant_info->listener = listener; + participant_info->graph_guard_condition = graph_guard_condition; + + return participant_info; +fail: + if (graph_guard_condition) { + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(graph_guard_condition); + if (ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_shared_cpp", + "failed to destroy guard condition during error handling"); + } + } + rmw_free(listener); + if (participant) { + Domain::removeParticipant(participant); + } + return nullptr; +} + +#define ROS_DOMAIN_ID_VAR_NAME "ROS_DOMAIN_ID" + +CustomParticipantInfo * +rmw_fastrtps_shared_cpp::create_participant( + const char * identifier, + size_t domain_id, + const rmw_security_options_t * security_options, + bool localhost_only, + rmw_dds_common::Context * common_context) +{ + if (!security_options) { + RMW_SET_ERROR_MSG("security_options is null"); + return nullptr; + } + ParticipantAttributes participantAttrs; + + // Load default XML profile. + Domain::getDefaultParticipantAttributes(participantAttrs); + + participantAttrs.rtps.builtin.domainId = static_cast(domain_id); + + if (localhost_only) { + Locator_t local_network_interface_locator; + static const std::string local_ip_name("127.0.0.1"); + local_network_interface_locator.kind = 1; + local_network_interface_locator.port = 0; + IPLocator::setIPv4(local_network_interface_locator, local_ip_name); + participantAttrs.rtps.builtin.metatrafficUnicastLocatorList.push_back( + local_network_interface_locator); + participantAttrs.rtps.builtin.initialPeersList.push_back(local_network_interface_locator); + } + + participantAttrs.rtps.builtin.domainId = static_cast(domain_id); + // // since the participant name is not part of the DDS spec + // participantAttrs.rtps.setName(name); + + bool leave_middleware_default_qos = false; + const char * env_value; + const char * error_str; + error_str = rcutils_get_env("RMW_FASTRTPS_USE_QOS_FROM_XML", &env_value); + if (error_str != NULL) { + RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_shared_cpp", "Error getting env var: %s\n", error_str); + return nullptr; + } + if (env_value != nullptr) { + leave_middleware_default_qos = strcmp(env_value, "1") == 0; + } + // allow reallocation to support discovery messages bigger than 5000 bytes + if (!leave_middleware_default_qos) { + participantAttrs.rtps.builtin.readerHistoryMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + participantAttrs.rtps.builtin.writerHistoryMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + if (security_options->security_root_path) { + // if security_root_path provided, try to find the key and certificate files +#if HAVE_SECURITY + std::array security_files_paths; + + if (get_security_file_paths(security_files_paths, security_options->security_root_path)) { + eprosima::fastrtps::rtps::PropertyPolicy property_policy; + using Property = eprosima::fastrtps::rtps::Property; + property_policy.properties().emplace_back( + Property("dds.sec.auth.plugin", "builtin.PKI-DH")); + property_policy.properties().emplace_back( + Property( + "dds.sec.auth.builtin.PKI-DH.identity_ca", security_files_paths[0])); + property_policy.properties().emplace_back( + Property( + "dds.sec.auth.builtin.PKI-DH.identity_certificate", security_files_paths[1])); + property_policy.properties().emplace_back( + Property( + "dds.sec.auth.builtin.PKI-DH.private_key", security_files_paths[2])); + property_policy.properties().emplace_back( + Property("dds.sec.crypto.plugin", "builtin.AES-GCM-GMAC")); + + property_policy.properties().emplace_back( + Property( + "dds.sec.access.plugin", "builtin.Access-Permissions")); + property_policy.properties().emplace_back( + Property( + "dds.sec.access.builtin.Access-Permissions.permissions_ca", security_files_paths[3])); + property_policy.properties().emplace_back( + Property( + "dds.sec.access.builtin.Access-Permissions.governance", security_files_paths[4])); + property_policy.properties().emplace_back( + Property( + "dds.sec.access.builtin.Access-Permissions.permissions", security_files_paths[5])); + + participantAttrs.rtps.properties = property_policy; + } else if (security_options->enforce_security) { + RMW_SET_ERROR_MSG("couldn't find all security files!"); + return nullptr; + } +#else + RMW_SET_ERROR_MSG( + "This Fast-RTPS version doesn't have the security libraries\n" + "Please compile Fast-RTPS using the -DSECURITY=ON CMake option"); + return nullptr; +#endif + } + return __create_participant( + identifier, + participantAttrs, + leave_middleware_default_qos, + common_context); +} + +rmw_ret_t +rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant_info) +{ + rmw_ret_t result_ret = RMW_RET_OK; + if (!participant_info) { + RMW_SET_ERROR_MSG("participant_info is null"); + return RMW_RET_ERROR; + } + Domain::removeParticipant(participant_info->participant); + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + participant_info->graph_guard_condition)) + { + RMW_SET_ERROR_MSG("failed to destroy graph guard condition"); + result_ret = RMW_RET_ERROR; + } + delete participant_info->listener; + participant_info->listener = nullptr; + delete participant_info; + return result_ret; +} diff --git a/rmw_fastrtps_shared_cpp/src/publisher.cpp b/rmw_fastrtps_shared_cpp/src/publisher.cpp new file mode 100644 index 000000000..9dc3eb3d7 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/publisher.cpp @@ -0,0 +1,69 @@ +// 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 "fastrtps/Domain.h" +#include "fastrtps/participant/Participant.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/publisher.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +destroy_publisher( + const char * identifier, + CustomParticipantInfo * participant_info, + rmw_publisher_t * publisher) +{ + if (!publisher) { + RMW_SET_ERROR_MSG("publisher handle is null"); + return RMW_RET_ERROR; + } + if (publisher->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("publisher handle not from this implementation"); + return RMW_RET_ERROR; + } + if (!participant_info) { + RMW_SET_ERROR_MSG("participant_info is null"); + return RMW_RET_ERROR; + } + + auto info = static_cast(publisher->data); + if (info != nullptr) { + if (info->publisher_ != nullptr) { + Domain::removePublisher(info->publisher_); + } + delete info->listener_; + if (info->type_support_ != nullptr) { + Participant * participant = participant_info->participant; + _unregister_type(participant, info->type_support_); + } + delete info; + } + rmw_free(const_cast(publisher->topic_name)); + publisher->topic_name = nullptr; + rmw_publisher_free(publisher); + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index e6ca5a20c..40645e5ec 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index b9f77310b..ec5237216 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -24,6 +24,7 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" using Domain = eprosima::fastrtps::Domain; @@ -48,7 +49,33 @@ __rmw_destroy_client( return RMW_RET_ERROR; } + auto common_context = static_cast(node->context->impl->common); auto info = static_cast(client->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->request_publisher_->getGuid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->response_subscriber_->getGuid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( + gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + } + if (info != nullptr) { if (info->response_subscriber_ != nullptr) { Domain::removeSubscriber(info->response_subscriber_); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_count.cpp b/rmw_fastrtps_shared_cpp/src/rmw_count.cpp index 0da58e436..902dc3776 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_count.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_count.cpp @@ -24,10 +24,12 @@ #include "rmw/rmw.h" #include "rmw/types.h" -#include "demangle.hpp" +#include "rmw_dds_common/context.hpp" + #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" namespace rmw_fastrtps_shared_cpp { @@ -39,53 +41,17 @@ __rmw_count_publishers( const char * topic_name, size_t * count) { - // safechecks - if (!node) { RMW_SET_ERROR_MSG("null node handle"); - return RMW_RET_ERROR; + return RMW_RET_INVALID_ARGUMENT; } - // Get participant pointer from node if (node->implementation_identifier != identifier) { RMW_SET_ERROR_MSG("node handle not from this implementation"); - return RMW_RET_ERROR; - } - - - auto ros_prefixes = _get_all_ros_prefixes(); - - // Build the list of all possible topic FQDN - std::vector topic_fqdns; - topic_fqdns.push_back(topic_name); - if (topic_name[0] == '/') { - std::for_each( - ros_prefixes.begin(), ros_prefixes.end(), - [&topic_fqdns, &topic_name](const std::string & prefix) { - topic_fqdns.push_back(prefix + topic_name); - }); - } - - auto impl = static_cast(node->data); - *count = 0; - ::ParticipantListener * slave_target = impl->listener; - { - std::lock_guard guard(slave_target->writer_topic_cache.getMutex()); - // Search and sum up the publisher counts - auto & topic_types = slave_target->writer_topic_cache().getTopicToTypes(); - for (const auto & topic_fqdn : topic_fqdns) { - const auto & it = topic_types.find(topic_fqdn); - if (it != topic_types.end()) { - *count += it->second.size(); - } - } + return RMW_RET_INVALID_ARGUMENT; } - - RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_shared_cpp", - "looking for publisher topic: %s, number of matches: %zu", - topic_name, *count); - - return RMW_RET_OK; + auto common_context = static_cast(node->context->impl->common); + const std::string mangled_topic_name = _mangle_topic_name(ros_topic_prefix, topic_name); + return common_context->graph_cache.get_writer_count(mangled_topic_name, count); } rmw_ret_t @@ -95,52 +61,16 @@ __rmw_count_subscribers( const char * topic_name, size_t * count) { - // safechecks - if (!node) { RMW_SET_ERROR_MSG("null node handle"); - return RMW_RET_ERROR; + return RMW_RET_INVALID_ARGUMENT; } - // Get participant pointer from node if (node->implementation_identifier != identifier) { RMW_SET_ERROR_MSG("node handle not from this implementation"); - return RMW_RET_ERROR; - } - - - auto ros_prefixes = _get_all_ros_prefixes(); - - // Build the list of all possible topic FQDN - std::vector topic_fqdns; - topic_fqdns.push_back(topic_name); - if (topic_name[0] == '/') { - std::for_each( - ros_prefixes.begin(), ros_prefixes.end(), - [&topic_fqdns, &topic_name](const std::string & prefix) { - topic_fqdns.push_back(prefix + topic_name); - }); + return RMW_RET_INVALID_ARGUMENT; } - - CustomParticipantInfo * impl = static_cast(node->data); - *count = 0; - ::ParticipantListener * slave_target = impl->listener; - { - std::lock_guard guard(slave_target->reader_topic_cache.getMutex()); - // Search and sum up the subscriber counts - auto & topic_types = slave_target->reader_topic_cache().getTopicToTypes(); - for (const auto & topic_fqdn : topic_fqdns) { - const auto & it = topic_types.find(topic_fqdn); - if (it != topic_types.end()) { - *count += it->second.size(); - } - } - } - - RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_shared_cpp", - "looking for subscriber topic: %s, number of matches: %zu", - topic_name, *count); - - return RMW_RET_OK; + auto common_context = static_cast(node->context->impl->common); + const std::string mangled_topic_name = _mangle_topic_name(ros_topic_prefix, topic_name); + return common_context->graph_cache.get_reader_count(mangled_topic_name, count); } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp index 7ca05d892..f8bc7b32e 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -24,9 +24,11 @@ #include "rmw/topic_endpoint_info.h" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" -#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +#include "rmw_dds_common/graph_cache.hpp" #include "demangle.hpp" @@ -75,246 +77,38 @@ _validate_params( return RMW_RET_OK; } -std::vector -_get_topic_fqdns(const char * topic_name, bool no_mangle) -{ - std::vector topic_fqdns; - topic_fqdns.push_back(topic_name); - if (!no_mangle) { - auto ros_prefixes = _get_all_ros_prefixes(); - // Build the list of all possible topic FQDN - std::for_each( - ros_prefixes.begin(), ros_prefixes.end(), - [&topic_fqdns, &topic_name](const std::string & prefix) { - topic_fqdns.push_back(prefix + topic_name); - }); - } - return topic_fqdns; -} - -void -_handle_topic_endpoint_info_fini( - rmw_topic_endpoint_info_t * topic_endpoint_info, - rcutils_allocator_t * allocator) -{ - bool had_error = rmw_error_is_set(); - std::string error_string; - if (had_error) { - error_string = rmw_get_error_string().str; - } - rmw_reset_error(); - - rmw_ret_t ret = rmw_topic_endpoint_info_fini(topic_endpoint_info, allocator); - if (ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "rmw_topic_endpoint_info_fini failed: %s", - rmw_get_error_string().str); - rmw_reset_error(); - } - - if (had_error) { - RMW_SET_ERROR_MSG(error_string.c_str()); - } -} - -rmw_ret_t -_set_rmw_topic_endpoint_info( - rmw_topic_endpoint_info_t * topic_endpoint_info, - const GUID_t & participant_guid, - const char * node_name, - const char * node_namespace, - TopicData topic_data, - bool no_mangle, - bool is_publisher, - ::ParticipantListener * slave_target, - rcutils_allocator_t * allocator) -{ - static_assert( - sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, - "RMW_GID_STORAGE_SIZE insufficient to store the rmw_fastrtps_cpp GID implementation." - ); - rmw_ret_t ret; - // set endpoint type - if (is_publisher) { - ret = rmw_topic_endpoint_info_set_endpoint_type(topic_endpoint_info, RMW_ENDPOINT_PUBLISHER); - } else { - ret = rmw_topic_endpoint_info_set_endpoint_type(topic_endpoint_info, RMW_ENDPOINT_SUBSCRIPTION); - } - if (ret != RMW_RET_OK) { - return ret; - } - // set endpoint gid - uint8_t rmw_gid[RMW_GID_STORAGE_SIZE] = {}; - rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( - topic_data.entity_guid, - rmw_gid); - ret = rmw_topic_endpoint_info_set_gid( - topic_endpoint_info, - rmw_gid, - sizeof(eprosima::fastrtps::rtps::GUID_t)); - if (ret != RMW_RET_OK) { - return ret; - } - // set qos profile - ret = rmw_topic_endpoint_info_set_qos_profile(topic_endpoint_info, &topic_data.qos_profile); - if (ret != RMW_RET_OK) { - return ret; - } - // set topic type - std::string type_name = - no_mangle ? topic_data.topic_type : _demangle_if_ros_type(topic_data.topic_type); - ret = rmw_topic_endpoint_info_set_topic_type(topic_endpoint_info, type_name.c_str(), allocator); - if (ret != RMW_RET_OK) { - return ret; - } - // Check if this participant is the same as the node that is passed - if (topic_data.participant_guid == participant_guid) { - ret = rmw_topic_endpoint_info_set_node_name(topic_endpoint_info, node_name, allocator); - if (ret != RMW_RET_OK) { - return ret; - } - ret = - rmw_topic_endpoint_info_set_node_namespace(topic_endpoint_info, node_namespace, allocator); - if (ret != RMW_RET_OK) { - return ret; - } - return ret; - } - // This means that this discovered participant is not associated with the passed node - // and hence we must find its name and namespace from the discovered_names and - // discovered_namespace maps - // set node name - { // Scope for lock guard - std::lock_guard guard(slave_target->names_mutex_); - const auto & d_name_it = slave_target->discovered_names.find(topic_data.participant_guid); - if (d_name_it != slave_target->discovered_names.end()) { - ret = rmw_topic_endpoint_info_set_node_name( - topic_endpoint_info, d_name_it->second.c_str(), allocator); - } else { - ret = rmw_topic_endpoint_info_set_node_name( - topic_endpoint_info, "_NODE_NAME_UNKNOWN_", allocator); - } - if (ret != RMW_RET_OK) { - return ret; - } - // set node namespace - const auto & d_namespace_it = - slave_target->discovered_namespaces.find(topic_data.participant_guid); - if (d_namespace_it != slave_target->discovered_namespaces.end()) { - ret = rmw_topic_endpoint_info_set_node_namespace( - topic_endpoint_info, d_namespace_it->second.c_str(), allocator); - } else { - ret = rmw_topic_endpoint_info_set_node_namespace( - topic_endpoint_info, "_NODE_NAMESPACE_UNKNOWN_", allocator); - } - } - return ret; -} - rmw_ret_t -_get_info_by_topic( +__rmw_get_publishers_info_by_topic( const char * identifier, const rmw_node_t * node, rcutils_allocator_t * allocator, const char * topic_name, bool no_mangle, - bool is_publisher, - rmw_topic_endpoint_info_array_t * participants_info) + rmw_topic_endpoint_info_array_t * publishers_info) { rmw_ret_t ret = _validate_params( identifier, node, allocator, topic_name, - participants_info); + publishers_info); if (ret != RMW_RET_OK) { return ret; } - - const auto & topic_fqdns = _get_topic_fqdns(topic_name, no_mangle); - auto impl = static_cast(node->data); - // The GUID of the participant associated with this node - const auto & participant_guid = impl->participant->getGuid(); - const auto & node_name = node->name; - const auto & node_namespace = node->namespace_; - ::ParticipantListener * slave_target = impl->listener; - auto & topic_cache = - is_publisher ? slave_target->writer_topic_cache : slave_target->reader_topic_cache; - { - std::lock_guard guard(topic_cache.getMutex()); - const auto & topic_name_to_data = topic_cache().getTopicNameToTopicData(); - std::vector topic_endpoint_info_vector; - for (const auto & topic_name : topic_fqdns) { - const auto it = topic_name_to_data.find(topic_name); - if (it != topic_name_to_data.end()) { - for (const auto & data : it->second) { - rmw_topic_endpoint_info_t topic_endpoint_info = - rmw_get_zero_initialized_topic_endpoint_info(); - rmw_ret_t ret = _set_rmw_topic_endpoint_info( - &topic_endpoint_info, - participant_guid, - node_name, - node_namespace, - data, - no_mangle, - is_publisher, - slave_target, - allocator); - if (ret != RMW_RET_OK) { - // Free topic_endpoint_info - _handle_topic_endpoint_info_fini(&topic_endpoint_info, allocator); - // Free all the space allocated to the previous topic_endpoint_infos - for (auto & tinfo : topic_endpoint_info_vector) { - _handle_topic_endpoint_info_fini(&tinfo, allocator); - } - return ret; - } - // add rmw_topic_endpoint_info_t to a vector - topic_endpoint_info_vector.push_back(topic_endpoint_info); - } - } - } - - // add all the elements from the vector to rmw_topic_endpoint_info_array_t - auto count = topic_endpoint_info_vector.size(); - ret = rmw_topic_endpoint_info_array_init_with_size(participants_info, count, allocator); - if (ret != RMW_RET_OK) { - rmw_error_string_t error_message = rmw_get_error_string(); - rmw_reset_error(); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "rmw_topic_endpoint_info_array_init_with_size failed to allocate memory: %s", - error_message.str); - // Free all the space allocated to the previous topic_endpoint_infos - for (auto & tinfo : topic_endpoint_info_vector) { - _handle_topic_endpoint_info_fini(&tinfo, allocator); - } - return ret; - } - for (auto i = 0u; i < count; i++) { - participants_info->info_array[i] = topic_endpoint_info_vector.at(i); - } + auto common_context = static_cast(node->context->impl->common); + rmw_dds_common::GraphCache & graph_cache = common_context->graph_cache; + std::string mangled_topic_name = topic_name; + DemangleFunction demangle_type = _identity_demangle; + if (!no_mangle) { + mangled_topic_name = _mangle_topic_name(ros_topic_prefix, topic_name); + demangle_type = _demangle_if_ros_type; } - return RMW_RET_OK; -} -rmw_ret_t -__rmw_get_publishers_info_by_topic( - const char * identifier, - const rmw_node_t * node, - rcutils_allocator_t * allocator, - const char * topic_name, - bool no_mangle, - rmw_topic_endpoint_info_array_t * publishers_info) -{ - return _get_info_by_topic( - identifier, - node, - allocator, + return graph_cache.get_writers_info_by_topic( topic_name, - no_mangle, - true, /*is_publisher*/ + demangle_type, + allocator, publishers_info); } @@ -327,13 +121,28 @@ __rmw_get_subscriptions_info_by_topic( bool no_mangle, rmw_topic_endpoint_info_array_t * subscriptions_info) { - return _get_info_by_topic( + rmw_ret_t ret = _validate_params( identifier, node, allocator, topic_name, - no_mangle, - false, /*is_publisher*/ + subscriptions_info); + if (ret != RMW_RET_OK) { + return ret; + } + auto common_context = static_cast(node->context->impl->common); + rmw_dds_common::GraphCache & graph_cache = common_context->graph_cache; + std::string mangled_topic_name = topic_name; + DemangleFunction demangle_type = _identity_demangle; + if (!no_mangle) { + mangled_topic_name = _mangle_topic_name(ros_topic_prefix, topic_name); + demangle_type = _demangle_if_ros_type; + } + + return graph_cache.get_readers_info_by_topic( + topic_name, + demangle_type, + allocator, subscriptions_info); } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp index be653928a..080e61cd0 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -13,6 +14,7 @@ // limitations under the License. #include +#include #include #include #include @@ -25,45 +27,21 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" -#include "fastrtps/config.h" -#include "fastrtps/Domain.h" -#include "fastrtps/rtps/common/Locator.h" -#include "fastrtps/participant/Participant.h" -#include "fastrtps/attributes/ParticipantAttributes.h" -#include "fastrtps/publisher/Publisher.h" -#include "fastrtps/attributes/PublisherAttributes.h" -#include "fastrtps/publisher/PublisherListener.h" -#include "fastrtps/subscriber/Subscriber.h" -#include "fastrtps/subscriber/SubscriberListener.h" -#include "fastrtps/subscriber/SampleInfo.h" -#include "fastrtps/attributes/SubscriberAttributes.h" -#include "fastrtps/utils/IPLocator.h" - -#include "fastrtps/rtps/RTPSDomain.h" - -#include "fastrtps/rtps/reader/RTPSReader.h" -#include "fastrtps/rtps/reader/StatefulReader.h" -#include "fastrtps/rtps/reader/ReaderListener.h" -#include "fastrtps/rtps/builtin/discovery/endpoint/EDPSimple.h" +#include "rmw_dds_common/context.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" -using Domain = eprosima::fastrtps::Domain; -using IPLocator = eprosima::fastrtps::rtps::IPLocator; -using Locator_t = eprosima::fastrtps::rtps::Locator_t; -using Participant = eprosima::fastrtps::Participant; -using ParticipantAttributes = eprosima::fastrtps::ParticipantAttributes; -using StatefulReader = eprosima::fastrtps::rtps::StatefulReader; namespace rmw_fastrtps_shared_cpp { rmw_node_t * -create_node( +__rmw_create_node( + rmw_context_t * context, const char * identifier, const char * name, - const char * namespace_, - ParticipantAttributes participantAttrs) + const char * namespace_) { if (!name) { RMW_SET_ERROR_MSG("name is null"); @@ -75,57 +53,9 @@ create_node( return nullptr; } - // Declare everything before beginning to create things. - ::ParticipantListener * listener = nullptr; - Participant * participant = nullptr; - rmw_guard_condition_t * graph_guard_condition = nullptr; - CustomParticipantInfo * node_impl = nullptr; rmw_node_t * node_handle = nullptr; - - graph_guard_condition = __rmw_create_guard_condition(identifier); - if (!graph_guard_condition) { - // error already set - goto fail; - } - - try { - listener = new ::ParticipantListener(graph_guard_condition); - } catch (std::bad_alloc &) { - RMW_SET_ERROR_MSG("failed to allocate participant listener"); - goto fail; - } - - participant = Domain::createParticipant(participantAttrs, listener); - if (!participant) { - RMW_SET_ERROR_MSG("create_node() could not create participant"); - return nullptr; - } - - try { - node_impl = new CustomParticipantInfo(); - - node_impl->leave_middleware_default_qos = false; - const char * env_var = "RMW_FASTRTPS_USE_QOS_FROM_XML"; - // Check if the configuration from XML has been enabled from - // the RMW_FASTRTPS_USE_QOS_FROM_XML env variable. - char * config_env_val = nullptr; -#ifndef _WIN32 - config_env_val = getenv(env_var); - if (config_env_val != nullptr) { - node_impl->leave_middleware_default_qos = strcmp(config_env_val, "1") == 0; - } -#else - size_t config_env_val_size; - _dupenv_s(&config_env_val, &config_env_val_size, env_var); - if (config_env_val != nullptr) { - node_impl->leave_middleware_default_qos = strcmp(config_env_val, "1") == 0; - } - free(config_env_val); -#endif - } catch (std::bad_alloc &) { - RMW_SET_ERROR_MSG("failed to allocate node impl struct"); - goto fail; - } + auto common_context = static_cast(context->impl->common); + rmw_dds_common::GraphCache & graph_cache = common_context->graph_cache; node_handle = rmw_node_allocate(); if (!node_handle) { @@ -133,10 +63,7 @@ create_node( goto fail; } node_handle->implementation_identifier = identifier; - node_impl->participant = participant; - node_impl->listener = listener; - node_impl->graph_guard_condition = graph_guard_condition; - node_handle->data = node_impl; + node_handle->data = nullptr; node_handle->name = static_cast(rmw_allocate(sizeof(char) * strlen(name) + 1)); @@ -154,7 +81,26 @@ create_node( goto fail; } memcpy(const_cast(node_handle->namespace_), namespace_, strlen(namespace_) + 1); - + node_handle->context = context; + + { + // Though graph_cache methods are thread safe, both cache update and publishing have to also + // be atomic. + // If not, the following race condition is possible: + // node1-update-get-message / node2-update-get-message / node2-publish / node1-publish + // In that case, the last message published is not accurate. + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo participant_msg = + graph_cache.add_node(common_context->gid, name, namespace_); + if (RMW_RET_OK != __rmw_publish( + identifier, + common_context->pub, + static_cast(&participant_msg), + nullptr)) + { + goto fail; + } + } return node_handle; fail: if (node_handle) { @@ -164,184 +110,9 @@ create_node( node_handle->name = nullptr; } rmw_node_free(node_handle); - delete node_impl; - if (graph_guard_condition) { - rmw_ret_t ret = __rmw_destroy_guard_condition(graph_guard_condition); - if (ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_shared_cpp", - "failed to destroy guard condition during error handling"); - } - } - rmw_free(listener); - if (participant) { - Domain::removeParticipant(participant); - } return nullptr; } -bool -get_security_file_paths( - std::array & security_files_paths, const char * node_secure_root) -{ - // here assume only 6 files for security - const char * file_names[6] = { - "identity_ca.cert.pem", "cert.pem", "key.pem", - "permissions_ca.cert.pem", "governance.p7s", "permissions.p7s" - }; - size_t num_files = sizeof(file_names) / sizeof(char *); - - std::string file_prefix("file://"); - - for (size_t i = 0; i < num_files; i++) { - rcutils_allocator_t allocator = rcutils_get_default_allocator(); - char * file_path = rcutils_join_path(node_secure_root, file_names[i], allocator); - - if (!file_path) { - return false; - } - - if (rcutils_is_readable(file_path)) { - security_files_paths[i] = file_prefix + std::string(file_path); - } else { - allocator.deallocate(file_path, allocator.state); - return false; - } - - allocator.deallocate(file_path, allocator.state); - } - - return true; -} - -rmw_node_t * -__rmw_create_node( - const char * identifier, - const char * name, - const char * namespace_, - size_t domain_id, - const rmw_node_security_options_t * security_options, - bool localhost_only) -{ - if (!name) { - RMW_SET_ERROR_MSG("name is null"); - return nullptr; - } - if (!security_options) { - RMW_SET_ERROR_MSG("security_options is null"); - return nullptr; - } - - ParticipantAttributes participantAttrs; - - // Load default XML profile. - Domain::getDefaultParticipantAttributes(participantAttrs); - - participantAttrs.rtps.builtin.domainId = static_cast(domain_id); - // since the participant name is not part of the DDS spec - participantAttrs.rtps.setName(name); - - if (localhost_only) { - Locator_t local_network_interface_locator; - static const std::string local_ip_name("127.0.0.1"); - local_network_interface_locator.kind = 1; - local_network_interface_locator.port = 0; - IPLocator::setIPv4(local_network_interface_locator, local_ip_name); - participantAttrs.rtps.builtin.metatrafficUnicastLocatorList.push_back( - local_network_interface_locator); - participantAttrs.rtps.builtin.initialPeersList.push_back(local_network_interface_locator); - } - bool leave_middleware_default_qos = false; - const char * env_var = "RMW_FASTRTPS_USE_QOS_FROM_XML"; - // Check if the configuration from XML has been enabled from - // the RMW_FASTRTPS_USE_QOS_FROM_XML env variable. - char * config_env_val = nullptr; -#ifndef _WIN32 - config_env_val = getenv(env_var); - if (config_env_val != nullptr) { - leave_middleware_default_qos = strcmp(config_env_val, "1") == 0; - } -#else - size_t config_env_val_size; - _dupenv_s(&config_env_val, &config_env_val_size, env_var); - if (config_env_val != nullptr) { - leave_middleware_default_qos = strcmp(config_env_val, "1") == 0; - } - free(config_env_val); -#endif - - // allow reallocation to support discovery messages bigger than 5000 bytes - if (!leave_middleware_default_qos) { - participantAttrs.rtps.builtin.readerHistoryMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - participantAttrs.rtps.builtin.writerHistoryMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - } - - size_t length = strlen(name) + strlen("name=;") + - strlen(namespace_) + strlen("namespace=;") + 1; - participantAttrs.rtps.userData.resize(length); - int written = snprintf( - reinterpret_cast(participantAttrs.rtps.userData.data()), - length, "name=%s;namespace=%s;", name, namespace_); - if (written < 0 || written > static_cast(length) - 1) { - RMW_SET_ERROR_MSG("failed to populate user_data buffer"); - return nullptr; - } - - if (security_options->security_root_path) { - // if security_root_path provided, try to find the key and certificate files -#if HAVE_SECURITY - std::array security_files_paths; - - if (get_security_file_paths(security_files_paths, security_options->security_root_path)) { - eprosima::fastrtps::rtps::PropertyPolicy property_policy; - using Property = eprosima::fastrtps::rtps::Property; - property_policy.properties().emplace_back( - Property("dds.sec.auth.plugin", "builtin.PKI-DH")); - property_policy.properties().emplace_back( - Property( - "dds.sec.auth.builtin.PKI-DH.identity_ca", - security_files_paths[0])); - property_policy.properties().emplace_back( - Property( - "dds.sec.auth.builtin.PKI-DH.identity_certificate", - security_files_paths[1])); - property_policy.properties().emplace_back( - Property( - "dds.sec.auth.builtin.PKI-DH.private_key", - security_files_paths[2])); - property_policy.properties().emplace_back( - Property("dds.sec.crypto.plugin", "builtin.AES-GCM-GMAC")); - - property_policy.properties().emplace_back( - Property( - "dds.sec.access.plugin", "builtin.Access-Permissions")); - property_policy.properties().emplace_back( - Property( - "dds.sec.access.builtin.Access-Permissions.permissions_ca", security_files_paths[3])); - property_policy.properties().emplace_back( - Property( - "dds.sec.access.builtin.Access-Permissions.governance", security_files_paths[4])); - property_policy.properties().emplace_back( - Property( - "dds.sec.access.builtin.Access-Permissions.permissions", security_files_paths[5])); - - participantAttrs.rtps.properties = property_policy; - } else if (security_options->enforce_security) { - RMW_SET_ERROR_MSG("couldn't find all security files!"); - return nullptr; - } -#else - RMW_SET_ERROR_MSG( - "This Fast-RTPS version doesn't have the security libraries\n" - "Please compile Fast-RTPS using the -DSECURITY=ON CMake option"); - return nullptr; -#endif - } - return create_node(identifier, name, namespace_, participantAttrs); -} - rmw_ret_t __rmw_destroy_node( const char * identifier, @@ -358,33 +129,28 @@ __rmw_destroy_node( return RMW_RET_ERROR; } - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return RMW_RET_ERROR; + auto common_context = static_cast(node->context->impl->common); + rmw_dds_common::GraphCache & graph_cache = common_context->graph_cache; + { + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo participant_msg = + graph_cache.remove_node(common_context->gid, node->name, node->namespace_); + result_ret = __rmw_publish( + identifier, + common_context->pub, + static_cast(&participant_msg), + nullptr); + if (RMW_RET_OK != result_ret) { + return result_ret; + } } - - Participant * participant = impl->participant; - - // Begin deleting things in the same order they were created in __rmw_create_node(). rmw_free(const_cast(node->name)); node->name = nullptr; rmw_free(const_cast(node->namespace_)); node->namespace_ = nullptr; rmw_node_free(node); - Domain::removeParticipant(participant); - - if (RMW_RET_OK != __rmw_destroy_guard_condition(impl->graph_guard_condition)) { - RMW_SET_ERROR_MSG("failed to destroy graph guard condition"); - result_ret = RMW_RET_ERROR; - } - - delete impl->listener; - impl->listener = nullptr; - delete impl; - - return result_ret; + return RMW_RET_OK; } rmw_ret_t @@ -392,31 +158,16 @@ __rmw_node_assert_liveliness( const char * identifier, const rmw_node_t * node) { - RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - node, - node->implementation_identifier, - identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - - auto node_info = static_cast(node->data); - if (nullptr == node_info) { - RMW_SET_ERROR_MSG("node info handle is null"); - return RMW_RET_ERROR; - } - if (nullptr == node_info->participant) { - RMW_SET_ERROR_MSG("node internal participant is invalid"); - return RMW_RET_ERROR; - } + (void)identifier; + (void)node; - node_info->participant->assert_liveliness(); - return RMW_RET_OK; + return RMW_RET_UNSUPPORTED; } const rmw_guard_condition_t * __rmw_node_get_graph_guard_condition(const rmw_node_t * node) { - auto impl = static_cast(node->data); + auto impl = static_cast(node->context->impl->participant_info); if (!impl) { RMW_SET_ERROR_MSG("node impl is null"); return nullptr; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp index 58e15f4d6..48e381f76 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2018 Amazon.com, Inc. or its affiliates. All Rights Reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -32,68 +33,20 @@ #include "rmw/names_and_types.h" #include "rmw/rmw.h" +#include "rmw_dds_common/context.hpp" + #include "demangle.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" - -#include "rmw_fastrtps_shared_cpp/topic_cache.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" namespace rmw_fastrtps_shared_cpp { constexpr char kLoggerTag[] = "rmw_fastrtps_shared_cpp"; -/** - * Get the guid that corresponds to the node and namespace. - * - * \param node to discover other participants with - * \param node_name of the desired node - * \param node_namespace of the desired node - * \param guid [out] result - * \return RMW_RET_ERROR if unable to find guid - * \return RMW_RET_OK if guid is available - */ -rmw_ret_t __get_guid_by_name( - const rmw_node_t * node, const char * node_name, - const char * node_namespace, GUID_t & guid) -{ - auto impl = static_cast(node->data); - if (strcmp(node->name, node_name) == 0 && strcmp(node->namespace_, node_namespace) == 0) { - guid = impl->participant->getGuid(); - } else { - std::set nodes_in_desired_namespace; - std::lock_guard guard(impl->listener->names_mutex_); - - auto namespaces = impl->listener->discovered_namespaces; - for (auto & guid_to_namespace : impl->listener->discovered_namespaces) { - if (guid_to_namespace.second == node_namespace) { - nodes_in_desired_namespace.insert(guid_to_namespace.first); - } - } - - auto guid_node_pair = std::find_if( - impl->listener->discovered_names.begin(), - impl->listener->discovered_names.end(), - [node_name, &nodes_in_desired_namespace](const std::pair & pair) { - return pair.second == node_name && - nodes_in_desired_namespace.find(pair.first) != nodes_in_desired_namespace.end(); - }); - - if (guid_node_pair == impl->listener->discovered_names.end()) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Node name not found: ns='%s', name='%s'", - node_namespace, - node_name - ); - return RMW_RET_NODE_NAME_NON_EXISTENT; - } - guid = guid_node_pair->first; - } - return RMW_RET_OK; -} - /** * Validate the input data of node_info_and_types functions. * @@ -141,189 +94,15 @@ rmw_ret_t __validate_input( return RMW_RET_OK; } -/** - * Access the slave Listeners, which are the ones that have the topicnamesandtypes member - * Get info from publisher and subscriber - * Combined results from the two lists - * - * \param topic_cache cache with topic information - * \param topics [out] resulting topics - * \param node_guid_ to find information for - * \param no_demangle true if demangling will not occur - */ -void -__accumulate_topics( - const LockedObject & topic_cache, - std::map> & topics, - const GUID_t & node_guid_, - bool no_demangle) -{ - std::lock_guard guard(topic_cache.getMutex()); - const auto & node_topics = topic_cache().getParticipantToTopics().find(node_guid_); - if (node_topics == topic_cache().getParticipantToTopics().end()) { - RCUTILS_LOG_DEBUG_NAMED( - kLoggerTag, - "No topics found for node"); - return; - } - for (auto & topic_pair : node_topics->second) { - if (!no_demangle && _get_ros_prefix_if_exists(topic_pair.first) != ros_topic_prefix) { - // if we are demangling and this is not prefixed with rt/, skip it - continue; - } - RCUTILS_LOG_DEBUG_NAMED( - kLoggerTag, - "accumulate_topics: Found topic %s", - topic_pair.first.c_str()); - - topics[topic_pair.first].insert( - topic_pair.second.begin(), topic_pair.second.end()); - } -} +using GetNamesAndTypesByNodeFunction = rmw_ret_t (*)( + rmw_dds_common::Context *, + const std::string &, + const std::string &, + DemangleFunction, + DemangleFunction, + rcutils_allocator_t *, + rmw_names_and_types_t *); -/** - * Copy topic data to results - * - * \param topics to copy over - * \param allocator to use - * \param no_demangle true if demangling will not occur - * \param topic_names_and_types [out] final rmw result - * \return RMW_RET_OK if successful - */ -rmw_ret_t -__copy_data_to_results( - const std::map> & topics, - rcutils_allocator_t * allocator, - bool no_demangle, - rmw_names_and_types_t * topic_names_and_types) -{ - // Copy data to results handle - if (!topics.empty()) { - // Setup string array to store names - rmw_ret_t rmw_ret = rmw_names_and_types_init(topic_names_and_types, topics.size(), allocator); - if (rmw_ret != RMW_RET_OK) { - return rmw_ret; - } - // Setup cleanup function, in case of failure below - auto fail_cleanup = [&topic_names_and_types]() { - rmw_ret_t rmw_ret = rmw_names_and_types_fini(topic_names_and_types); - if (rmw_ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - kLoggerTag, - "error during report of error: %s", rmw_get_error_string().str); - } - }; - // Setup demangling functions based on no_demangle option - auto demangle_topic = _demangle_if_ros_topic; - auto demangle_type = _demangle_if_ros_type; - if (no_demangle) { - auto noop = [](const std::string & in) { - return in; - }; - demangle_topic = noop; - demangle_type = noop; - } - // For each topic, store the name, initialize the string array for types, and store all types - size_t index = 0; - for (const auto & topic_n_types : topics) { - // Duplicate and store the topic_name - char * topic_name = rcutils_strdup(demangle_topic(topic_n_types.first).c_str(), *allocator); - if (!topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for topic name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - topic_names_and_types->names.data[index] = topic_name; - // Setup storage for types - { - rcutils_ret_t rcutils_ret = rcutils_string_array_init( - &topic_names_and_types->types[index], - topic_n_types.second.size(), - allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RMW_SET_ERROR_MSG(rcutils_get_error_string().str); - fail_cleanup(); - return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); - } - } - // Duplicate and store each type for the topic - size_t type_index = 0; - for (const auto & type : topic_n_types.second) { - char * type_name = rcutils_strdup(demangle_type(type).c_str(), *allocator); - if (!type_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for type name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - topic_names_and_types->types[index].data[type_index] = type_name; - ++type_index; - } // for each type - ++index; - } // for each topic - } - return RMW_RET_OK; -} - -void -__log_debug_information(const CustomParticipantInfo & impl) -{ - if (rcutils_logging_logger_is_enabled_for(kLoggerTag, RCUTILS_LOG_SEVERITY_DEBUG)) { - { - auto & topic_cache = impl.listener->writer_topic_cache; - std::lock_guard guard(topic_cache.getMutex()); - std::stringstream map_ss; - map_ss << topic_cache(); - RCUTILS_LOG_DEBUG_NAMED( - kLoggerTag, - "Publisher Topic cache is: %s", map_ss.str().c_str()); - } - { - auto & topic_cache = impl.listener->reader_topic_cache; - std::lock_guard guard(topic_cache.getMutex()); - std::stringstream map_ss; - map_ss << topic_cache(); - RCUTILS_LOG_DEBUG_NAMED( - kLoggerTag, - "Subscriber Topic cache is: %s", map_ss.str().c_str()); - } - { - std::stringstream ss; - std::lock_guard guard(impl.listener->names_mutex_); - for (auto & node_pair : impl.listener->discovered_names) { - ss << node_pair.first << " : " << node_pair.second << " "; - } - RCUTILS_LOG_DEBUG_NAMED(kLoggerTag, "Discovered names: %s", ss.str().c_str()); - } - { - std::stringstream ss; - std::lock_guard guard(impl.listener->names_mutex_); - for (auto & node_pair : impl.listener->discovered_namespaces) { - ss << node_pair.first << " : " << node_pair.second << " "; - } - RCUTILS_LOG_DEBUG_NAMED(kLoggerTag, "Discovered namespaces: %s", ss.str().c_str()); - } - } -} - -/** - * Function to abstract which topic_cache to use when gathering information. - */ -typedef std::function&(CustomParticipantInfo & participant_info)> - RetrieveCache; - -/** - * Get topic names and types for the specific node_name and node_namespace requested. - * - * \param identifier corresponding to the input node - * \param node to use for discovery - * \param allocator for returned value - * \param node_name to search - * \param node_namespace to search - * \param no_demangle true if the topics should not be demangled - * \param retrieve_cache_func getter for topic cache - * \param topic_names_and_types result - * \return RMW_RET_OK if successful - */ rmw_ret_t __rmw_get_topic_names_and_types_by_node( const char * identifier, @@ -331,8 +110,10 @@ __rmw_get_topic_names_and_types_by_node( rcutils_allocator_t * allocator, const char * node_name, const char * node_namespace, + DemangleFunction demangle_topic, + DemangleFunction demangle_type, bool no_demangle, - RetrieveCache & retrieve_cache_func, + GetNamesAndTypesByNodeFunction get_names_and_types_by_node, rmw_names_and_types_t * topic_names_and_types) { rmw_ret_t valid_input = __validate_input( @@ -340,42 +121,63 @@ __rmw_get_topic_names_and_types_by_node( if (valid_input != RMW_RET_OK) { return valid_input; } - RCUTILS_LOG_DEBUG_NAMED(kLoggerTag, "rmw_get_subscriber_names_and_types_by_node"); - auto impl = static_cast(node->data); + auto common_context = static_cast(node->context->impl->common); - __log_debug_information(*impl); - - GUID_t guid; - rmw_ret_t valid_guid = __get_guid_by_name(node, node_name, node_namespace, guid); - if (valid_guid != RMW_RET_OK) { - return valid_guid; + if (no_demangle) { + demangle_topic = _identity_demangle; + demangle_type = _identity_demangle; } - std::map> topics; - __accumulate_topics(retrieve_cache_func(*impl), topics, guid, no_demangle); - return __copy_data_to_results(topics, allocator, no_demangle, topic_names_and_types); + + return get_names_and_types_by_node( + common_context, + node_name, + node_namespace, + demangle_topic, + demangle_type, + allocator, + topic_names_and_types); } rmw_ret_t -__rmw_get_subscriber_names_and_types_by_node( - const char * identifier, - const rmw_node_t * node, +__get_reader_names_and_types_by_node( + rmw_dds_common::Context * common_context, + const std::string & node_name, + const std::string & node_namespace, + DemangleFunction demangle_topic, + DemangleFunction demangle_type, rcutils_allocator_t * allocator, - const char * node_name, - const char * node_namespace, - bool no_demangle, rmw_names_and_types_t * topic_names_and_types) { - RetrieveCache retrieve_sub_cache = - [](CustomParticipantInfo & participant_info) -> const LockedObject & { - return participant_info.listener->reader_topic_cache; - }; - return __rmw_get_topic_names_and_types_by_node( - identifier, node, allocator, node_name, - node_namespace, no_demangle, retrieve_sub_cache, topic_names_and_types); + return common_context->graph_cache.get_reader_names_and_types_by_node( + node_name, + node_namespace, + demangle_topic, + demangle_type, + allocator, + topic_names_and_types); } rmw_ret_t -__rmw_get_publisher_names_and_types_by_node( +__get_writer_names_and_types_by_node( + rmw_dds_common::Context * common_context, + const std::string & node_name, + const std::string & node_namespace, + DemangleFunction demangle_topic, + DemangleFunction demangle_type, + rcutils_allocator_t * allocator, + rmw_names_and_types_t * topic_names_and_types) +{ + return common_context->graph_cache.get_writer_names_and_types_by_node( + node_name, + node_namespace, + demangle_topic, + demangle_type, + allocator, + topic_names_and_types); +} + +rmw_ret_t +__rmw_get_subscriber_names_and_types_by_node( const char * identifier, const rmw_node_t * node, rcutils_allocator_t * allocator, @@ -384,125 +186,40 @@ __rmw_get_publisher_names_and_types_by_node( bool no_demangle, rmw_names_and_types_t * topic_names_and_types) { - RetrieveCache retrieve_pub_cache = - [](CustomParticipantInfo & participant_info) -> const LockedObject & { - return participant_info.listener->writer_topic_cache; - }; return __rmw_get_topic_names_and_types_by_node( - identifier, node, allocator, node_name, - node_namespace, no_demangle, retrieve_pub_cache, topic_names_and_types); + identifier, + node, + allocator, + node_name, + node_namespace, + _demangle_ros_topic_from_topic, + _demangle_if_ros_type, + no_demangle, + __get_reader_names_and_types_by_node, + topic_names_and_types); } -static rmw_ret_t -__get_service_names_and_types_by_node( +__rmw_get_publisher_names_and_types_by_node( const char * identifier, const rmw_node_t * node, rcutils_allocator_t * allocator, const char * node_name, const char * node_namespace, - rmw_names_and_types_t * service_names_and_types, - const char * topic_suffix) + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) { - const std::string topic_suffix_stdstr(topic_suffix); - rmw_ret_t valid_input = __validate_input( - identifier, node, allocator, node_name, node_namespace, service_names_and_types); - if (valid_input != RMW_RET_OK) { - return valid_input; - } - auto impl = static_cast(node->data); - __log_debug_information(*impl); - - GUID_t guid; - rmw_ret_t valid_guid = __get_guid_by_name(node, node_name, node_namespace, guid); - if (valid_guid != RMW_RET_OK) { - return valid_guid; - } - - std::map> services; - { - auto & topic_cache = impl->listener->reader_topic_cache; - std::lock_guard guard(topic_cache.getMutex()); - const auto & node_topics = topic_cache().getParticipantToTopics().find(guid); - if (node_topics != topic_cache().getParticipantToTopics().end()) { - for (auto & topic_pair : node_topics->second) { - std::string service_name = _demangle_service_from_topic(topic_pair.first); - if (service_name.empty()) { - // not a service - continue; - } - // Check if the topic suffix matches and is at the end of the name - const std::string & topic_name = topic_pair.first; - auto suffix_position = topic_name.rfind(topic_suffix_stdstr); - if (suffix_position == std::string::npos || - topic_name.length() - suffix_position - topic_suffix_stdstr.length() != 0) - { - continue; - } - - for (auto & itt : topic_pair.second) { - std::string service_type = _demangle_service_type_only(itt); - if (!service_type.empty()) { - services[service_name].insert(service_type); - } - } - } - } - } - if (services.empty()) { - return RMW_RET_OK; - } - // Setup string array to store names - rmw_ret_t rmw_ret = - rmw_names_and_types_init(service_names_and_types, services.size(), allocator); - if (rmw_ret != RMW_RET_OK) { - return rmw_ret; - } - // Setup cleanup function, in case of failure below - auto fail_cleanup = [&service_names_and_types]() { - rmw_ret_t rmw_ret = rmw_names_and_types_fini(service_names_and_types); - if (rmw_ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - kLoggerTag, - "error during report of error: %s", rmw_get_error_string().str); - } - }; - // For each service, store the name, initialize the string array for types, and store all types - size_t index = 0; - for (const auto & service_n_types : services) { - // Duplicate and store the service_name - char * service_name = rcutils_strdup(service_n_types.first.c_str(), *allocator); - if (!service_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for service name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - service_names_and_types->names.data[index] = service_name; - // Setup storage for types - rcutils_ret_t rcutils_ret = rcutils_string_array_init( - &service_names_and_types->types[index], - service_n_types.second.size(), - allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RMW_SET_ERROR_MSG(rcutils_get_error_string().str); - fail_cleanup(); - return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); - } - // Duplicate and store each type for the service - size_t type_index = 0; - for (const auto & type : service_n_types.second) { - char * type_name = rcutils_strdup(type.c_str(), *allocator); - if (!type_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for type name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - service_names_and_types->types[index].data[type_index] = type_name; - ++type_index; - } // for each type - ++index; - } // for each service - return RMW_RET_OK; + return __rmw_get_topic_names_and_types_by_node( + identifier, + node, + allocator, + node_name, + node_namespace, + _demangle_ros_topic_from_topic, + _demangle_if_ros_type, + no_demangle, + __get_writer_names_and_types_by_node, + topic_names_and_types); } rmw_ret_t @@ -514,14 +231,17 @@ __rmw_get_service_names_and_types_by_node( const char * node_namespace, rmw_names_and_types_t * service_names_and_types) { - return __get_service_names_and_types_by_node( + return __rmw_get_topic_names_and_types_by_node( identifier, node, allocator, node_name, node_namespace, - service_names_and_types, - "Request"); + _demangle_service_request_from_topic, + _demangle_service_type_only, + false, + __get_reader_names_and_types_by_node, + service_names_and_types); } rmw_ret_t @@ -533,14 +253,17 @@ __rmw_get_client_names_and_types_by_node( const char * node_namespace, rmw_names_and_types_t * service_names_and_types) { - return __get_service_names_and_types_by_node( + return __rmw_get_topic_names_and_types_by_node( identifier, node, allocator, node_name, node_namespace, - service_names_and_types, - "Reply"); + _demangle_service_reply_from_topic, + _demangle_service_type_only, + false, + __get_reader_names_and_types_by_node, + service_names_and_types); } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp index 4644edaf7..846ef7819 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp @@ -14,6 +14,8 @@ #include +#include "fastrtps/Domain.h" + #include "rcutils/allocator.h" #include "rcutils/logging_macros.h" #include "rcutils/strdup.h" @@ -25,10 +27,11 @@ #include "rmw/rmw.h" #include "rmw/sanity_checks.h" -#include "fastrtps/Domain.h" +#include "rmw_dds_common/context.hpp" -#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" using Participant = eprosima::fastrtps::Participant; @@ -51,65 +54,16 @@ __rmw_get_node_names( if (rmw_check_zero_rmw_string_array(node_namespaces) != RMW_RET_OK) { return RMW_RET_ERROR; } - - // Get participant pointer from node if (node->implementation_identifier != identifier) { RMW_SET_ERROR_MSG("node handle not from this implementation"); return RMW_RET_ERROR; } - auto impl = static_cast(node->data); - auto participant_names = impl->listener->get_discovered_names(); - auto participant_ns = impl->listener->get_discovered_namespaces(); - + auto common_context = static_cast(node->context->impl->common); rcutils_allocator_t allocator = rcutils_get_default_allocator(); - rcutils_ret_t rcutils_ret = - rcutils_string_array_init(node_names, participant_names.size() + 1, &allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RMW_SET_ERROR_MSG(rcutils_get_error_string().str); - goto fail; - } - - rcutils_ret = - rcutils_string_array_init(node_namespaces, participant_names.size() + 1, &allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RMW_SET_ERROR_MSG(rcutils_get_error_string().str); - goto fail; - } - - for (size_t i = 0; i < participant_names.size() + 1; ++i) { - if (0 == i) { - node_names->data[i] = rcutils_strdup(node->name, allocator); - node_namespaces->data[i] = rcutils_strdup(node->namespace_, allocator); - } else { - node_names->data[i] = rcutils_strdup(participant_names[i - 1].c_str(), allocator); - node_namespaces->data[i] = rcutils_strdup(participant_ns[i - 1].c_str(), allocator); - } - if (!node_names->data[i] || !node_namespaces->data[i]) { - RMW_SET_ERROR_MSG("failed to allocate memory for node name"); - goto fail; - } - } - return RMW_RET_OK; -fail: - if (node_names) { - rcutils_ret = rcutils_string_array_fini(node_names); - if (rcutils_ret != RCUTILS_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "failed to cleanup during error handling: %s", rcutils_get_error_string().str); - rcutils_reset_error(); - } - } - if (node_namespaces) { - rcutils_ret = rcutils_string_array_fini(node_namespaces); - if (rcutils_ret != RCUTILS_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "failed to cleanup during error handling: %s", rcutils_get_error_string().str); - rcutils_reset_error(); - } - } - return RMW_RET_BAD_ALLOC; + return common_context->graph_cache.get_node_names( + node_names, + node_namespaces, + &allocator); } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp index 5fad9caa4..0ee94394a 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -14,9 +15,6 @@ #include -#include "fastrtps/Domain.h" -#include "fastrtps/participant/Participant.h" - #include "rmw/allocators.h" #include "rmw/error_handling.h" #include "rmw/impl/cpp/macros.hpp" @@ -25,19 +23,18 @@ #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/publisher.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; - namespace rmw_fastrtps_shared_cpp { rmw_ret_t __rmw_destroy_publisher( const char * identifier, - rmw_node_t * node, + const rmw_node_t * node, rmw_publisher_t * publisher) { if (!node) { @@ -50,41 +47,30 @@ __rmw_destroy_publisher( return RMW_RET_ERROR; } - if (!publisher) { - RMW_SET_ERROR_MSG("publisher handle is null"); - return RMW_RET_ERROR; - } - - if (publisher->implementation_identifier != identifier) { - RMW_SET_ERROR_MSG("publisher handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto info = static_cast(publisher->data); - if (info != nullptr) { - if (info->publisher_ != nullptr) { - Domain::removePublisher(info->publisher_); - } - if (info->listener_ != nullptr) { - delete info->listener_; + auto common_context = static_cast(node->context->impl->common); + auto info = static_cast(publisher->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_writer( + info->publisher_gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + &msg, + nullptr); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; } - if (info->type_support_ != nullptr) { - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return RMW_RET_ERROR; - } - - Participant * participant = impl->participant; - _unregister_type(participant, info->type_support_); - } - delete info; } - rmw_free(const_cast(publisher->topic_name)); - publisher->topic_name = nullptr; - rmw_publisher_free(publisher); - return RMW_RET_OK; + auto participant_info = + static_cast(node->context->impl->participant_info); + return destroy_publisher( + identifier, + participant_info, + publisher); } rmw_ret_t diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index bfed95105..f69a4f245 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -30,11 +30,13 @@ #include "rmw/allocators.h" #include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" using Domain = eprosima::fastrtps::Domain; @@ -49,7 +51,6 @@ __rmw_destroy_service( rmw_node_t * node, rmw_service_t * service) { - (void)node; if (!service) { RMW_SET_ERROR_MSG("service handle is null"); return RMW_RET_ERROR; @@ -58,8 +59,37 @@ __rmw_destroy_service( RMW_SET_ERROR_MSG("publisher handle not from this implementation"); return RMW_RET_ERROR; } + if (!node) { + RMW_SET_ERROR_MSG("node handle is null"); + return RMW_RET_ERROR; + } + auto common_context = static_cast(node->context->impl->common); + auto info = static_cast(service->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->request_subscriber_->getGuid()); + common_context->graph_cache.dissociate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->response_publisher_->getGuid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_writer( + gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + &msg, + nullptr); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + } - CustomServiceInfo * info = static_cast(service->data); if (info != nullptr) { if (info->request_subscriber_ != nullptr) { Domain::removeSubscriber(info->request_subscriber_); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service_names_and_types.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service_names_and_types.cpp index a7dd7318e..a35520657 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service_names_and_types.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service_names_and_types.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -12,28 +13,26 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include -#include -#include "rcutils/strdup.h" +#include "rcutils/allocator.h" #include "rmw/allocators.h" -#include "rmw/convert_rcutils_ret_to_rmw_ret.h" #include "rmw/error_handling.h" #include "rmw/get_service_names_and_types.h" #include "rmw/names_and_types.h" -#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_dds_common/context.hpp" -#include "demangle.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" -#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" -#include "rmw_fastrtps_shared_cpp/topic_cache.hpp" +#include "demangle.hpp" namespace rmw_fastrtps_shared_cpp { + rmw_ret_t __rmw_get_service_names_and_types( const char * identifier, @@ -53,97 +52,17 @@ __rmw_get_service_names_and_types( if (ret != RMW_RET_OK) { return ret; } - - // Get participant pointer from node if (node->implementation_identifier != identifier) { RMW_SET_ERROR_MSG("node handle not from this implementation"); return RMW_RET_ERROR; } - auto impl = static_cast(node->data); + auto common_context = static_cast(node->context->impl->common); - // Access the slave Listeners, which are the ones that have the topicnamesandtypes member - // Get info from publisher and subscriber - // Combined results from the two lists - std::map> services; - - // Setup processing function, will be used with two maps - auto map_process = [&services](const LockedObject & topic_cache) { - std::lock_guard guard(topic_cache.getMutex()); - for (auto it : topic_cache().getTopicToTypes()) { - std::string service_name = _demangle_service_from_topic(it.first); - if (service_name.empty()) { - // not a service - continue; - } - for (auto & itt : it.second) { - std::string service_type = _demangle_service_type_only(itt); - if (!service_type.empty()) { - services[service_name].insert(service_type); - } - } - } - }; - - ::ParticipantListener * slave_target = impl->listener; - map_process(slave_target->reader_topic_cache); - map_process(slave_target->writer_topic_cache); - - // Fill out service_names_and_types - if (!services.empty()) { - // Setup string array to store names - rmw_ret_t rmw_ret = - rmw_names_and_types_init(service_names_and_types, services.size(), allocator); - if (rmw_ret != RMW_RET_OK) { - return rmw_ret; - } - // Setup cleanup function, in case of failure below - auto fail_cleanup = [&service_names_and_types]() { - rmw_ret_t rmw_ret = rmw_names_and_types_fini(service_names_and_types); - if (rmw_ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_shared_cpp", - "error during report of error: %s", rmw_get_error_string().str); - } - }; - // For each service, store the name, initialize the string array for types, and store all types - size_t index = 0; - for (const auto & service_n_types : services) { - // Duplicate and store the service_name - char * service_name = rcutils_strdup(service_n_types.first.c_str(), *allocator); - if (!service_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for service name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - service_names_and_types->names.data[index] = service_name; - // Setup storage for types - { - rcutils_ret_t rcutils_ret = rcutils_string_array_init( - &service_names_and_types->types[index], - service_n_types.second.size(), - allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RMW_SET_ERROR_MSG(rcutils_get_error_string().str); - fail_cleanup(); - return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); - } - } - // Duplicate and store each type for the service - size_t type_index = 0; - for (const auto & type : service_n_types.second) { - char * type_name = rcutils_strdup(type.c_str(), *allocator); - if (!type_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for type name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - service_names_and_types->types[index].data[type_index] = type_name; - ++type_index; - } // for each type - ++index; - } // for each service - } - return RMW_RET_OK; + return common_context->graph_cache.get_names_and_types( + _demangle_service_from_topic, + _demangle_service_type_only, + allocator, + service_names_and_types); } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp index fec5f9189..47e49e830 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service_server_is_available.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -24,9 +25,12 @@ #include "rmw/rmw.h" #include "rmw/types.h" +#include "rmw_dds_common/context.hpp" + #include "demangle.hpp" #include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" namespace rmw_fastrtps_shared_cpp { @@ -66,22 +70,17 @@ __rmw_service_server_is_available( auto pub_topic_name = client_info->request_publisher_->getAttributes().topic.getTopicName().to_string(); - auto pub_fqdn = _demangle_if_ros_topic(pub_topic_name); - auto sub_topic_name = client_info->response_subscriber_->getAttributes().topic.getTopicName().to_string(); - auto sub_fqdn = _demangle_if_ros_topic(sub_topic_name); - *is_available = false; + auto common_context = static_cast(node->context->impl->common); + size_t number_of_request_subscribers = 0; - rmw_ret_t ret = __rmw_count_subscribers( - identifier, - node, - pub_fqdn.c_str(), - &number_of_request_subscribers); + rmw_ret_t ret = + common_context->graph_cache.get_reader_count(pub_topic_name, &number_of_request_subscribers); if (ret != RMW_RET_OK) { - // error string already set + // error return ret; } if (0 == number_of_request_subscribers) { @@ -90,13 +89,10 @@ __rmw_service_server_is_available( } size_t number_of_response_publishers = 0; - ret = __rmw_count_publishers( - identifier, - node, - sub_fqdn.c_str(), - &number_of_response_publishers); + ret = + common_context->graph_cache.get_writer_count(sub_topic_name, &number_of_response_publishers); if (ret != RMW_RET_OK) { - // error string already set + // error return ret; } if (0 == number_of_response_publishers) { diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 5f2e6f852..e8258472b 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,9 +20,6 @@ #include "rmw/error_handling.h" #include "rmw/rmw.h" -#include "fastrtps/Domain.h" -#include "fastrtps/TopicDataType.h" -#include "fastrtps/participant/Participant.h" #include "fastrtps/subscriber/Subscriber.h" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" @@ -29,18 +27,16 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; - namespace rmw_fastrtps_shared_cpp { rmw_ret_t __rmw_destroy_subscription( const char * identifier, - rmw_node_t * node, + const rmw_node_t * node, rmw_subscription_t * subscription) { if (!node) { @@ -53,42 +49,30 @@ __rmw_destroy_subscription( return RMW_RET_ERROR; } - if (!subscription) { - RMW_SET_ERROR_MSG("subscription handle is null"); - return RMW_RET_ERROR; - } - - if (subscription->implementation_identifier != identifier) { - RMW_SET_ERROR_MSG("node handle not from this implementation"); - return RMW_RET_ERROR; - } - - auto info = static_cast(subscription->data); - - if (info != nullptr) { - if (info->subscriber_ != nullptr) { - Domain::removeSubscriber(info->subscriber_); - } - if (info->listener_ != nullptr) { - delete info->listener_; + auto common_context = static_cast(node->context->impl->common); + auto info = static_cast(subscription->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( + info->subscription_gid_, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; } - if (info->type_support_ != nullptr) { - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return RMW_RET_ERROR; - } - - Participant * participant = impl->participant; - _unregister_type(participant, info->type_support_); - } - delete info; } - rmw_free(const_cast(subscription->topic_name)); - subscription->topic_name = nullptr; - rmw_subscription_free(subscription); - return RMW_RET_OK; + auto participant_info = + static_cast(node->context->impl->participant_info); + return destroy_subscription( + identifier, + participant_info, + subscription); } rmw_ret_t diff --git a/rmw_fastrtps_shared_cpp/src/rmw_topic_names_and_types.cpp b/rmw_fastrtps_shared_cpp/src/rmw_topic_names_and_types.cpp index 7d5da6414..761a73143 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_topic_names_and_types.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_topic_names_and_types.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -12,36 +13,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include -#include -#include - #include "rcutils/allocator.h" -#include "rcutils/error_handling.h" -#include "rcutils/logging_macros.h" -#include "rcutils/strdup.h" -#include "rcutils/types.h" #include "rmw/allocators.h" -#include "rmw/convert_rcutils_ret_to_rmw_ret.h" #include "rmw/error_handling.h" #include "rmw/get_topic_names_and_types.h" #include "rmw/impl/cpp/macros.hpp" #include "rmw/names_and_types.h" -#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_dds_common/context.hpp" -#include "demangle.hpp" -#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" -#include "rmw_fastrtps_shared_cpp/topic_cache.hpp" +#include "demangle.hpp" namespace rmw_fastrtps_shared_cpp { + rmw_ret_t __rmw_get_topic_names_and_types( const char * identifier, @@ -58,108 +50,28 @@ __rmw_get_topic_names_and_types( RMW_SET_ERROR_MSG("null node handle"); return RMW_RET_INVALID_ARGUMENT; } - rmw_ret_t ret = rmw_names_and_types_check_zero(topic_names_and_types); if (ret != RMW_RET_OK) { return ret; } - - // Get participant pointer from node if (node->implementation_identifier != identifier) { RMW_SET_ERROR_MSG("node handle not from this implementation"); return RMW_RET_ERROR; } - auto impl = static_cast(node->data); - - // Access the slave Listeners, which are the ones that have the topicnamesandtypes member - // Get info from publisher and subscriber - // Combined results from the two lists - std::map> topics; - - // Setup processing function, will be used with two maps - auto map_process = - [&topics, no_demangle](const LockedObject & topic_cache) { - std::lock_guard guard(topic_cache.getMutex()); - for (auto it : topic_cache().getTopicToTypes()) { - if (!no_demangle && _get_ros_prefix_if_exists(it.first) != ros_topic_prefix) { - // if we are demangling and this is not prefixed with rt/, skip it - continue; - } - for (auto & itt : it.second) { - topics[it.first].insert(itt); - } - } - }; + DemangleFunction demangle_topic = _demangle_ros_topic_from_topic; + DemangleFunction demangle_type = _demangle_if_ros_type; - ::ParticipantListener * slave_target = impl->listener; - map_process(slave_target->reader_topic_cache); - map_process(slave_target->writer_topic_cache); - - // Copy data to results handle - if (!topics.empty()) { - // Setup string array to store names - rmw_ret_t rmw_ret = rmw_names_and_types_init(topic_names_and_types, topics.size(), allocator); - if (rmw_ret != RMW_RET_OK) { - return rmw_ret; - } - // Setup cleanup function, in case of failure below - auto fail_cleanup = [&topic_names_and_types]() { - rmw_ret_t rmw_ret = rmw_names_and_types_fini(topic_names_and_types); - if (rmw_ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_shared_cpp", - "error during report of error: %s", rmw_get_error_string().str); - } - }; - // Setup demangling functions based on no_demangle option - auto demangle_topic = _demangle_if_ros_topic; - auto demangle_type = _demangle_if_ros_type; - if (no_demangle) { - auto noop = [](const std::string & in) { - return in; - }; - demangle_topic = noop; - demangle_type = noop; - } - // For each topic, store the name, initialize the string array for types, and store all types - size_t index = 0; - for (const auto & topic_n_types : topics) { - // Duplicate and store the topic_name - char * topic_name = rcutils_strdup(demangle_topic(topic_n_types.first).c_str(), *allocator); - if (!topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for topic name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - topic_names_and_types->names.data[index] = topic_name; - // Setup storage for types - { - rcutils_ret_t rcutils_ret = rcutils_string_array_init( - &topic_names_and_types->types[index], - topic_n_types.second.size(), - allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RMW_SET_ERROR_MSG(rcutils_get_error_string().str); - fail_cleanup(); - return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); - } - } - // Duplicate and store each type for the topic - size_t type_index = 0; - for (const auto & type : topic_n_types.second) { - char * type_name = rcutils_strdup(demangle_type(type).c_str(), *allocator); - if (!type_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for type name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - topic_names_and_types->types[index].data[type_index] = type_name; - ++type_index; - } // for each type - ++index; - } // for each topic + if (no_demangle) { + demangle_topic = _identity_demangle; + demangle_type = _identity_demangle; } - return RMW_RET_OK; + auto common_context = static_cast(node->context->impl->common); + + return common_context->graph_cache.get_names_and_types( + demangle_topic, + demangle_type, + allocator, + topic_names_and_types); } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/subscription.cpp b/rmw_fastrtps_shared_cpp/src/subscription.cpp new file mode 100644 index 000000000..1899bfe46 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/subscription.cpp @@ -0,0 +1,78 @@ +// 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 "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "fastrtps/Domain.h" +#include "fastrtps/TopicDataType.h" +#include "fastrtps/participant/Participant.h" +#include "fastrtps/subscriber/Subscriber.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +destroy_subscription( + const char * identifier, + CustomParticipantInfo * participant_info, + rmw_subscription_t * subscription) +{ + if (!subscription) { + RMW_SET_ERROR_MSG("subscription handle is null"); + return RMW_RET_ERROR; + } + if (subscription->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("subscription handle not from this implementation"); + return RMW_RET_ERROR; + } + if (!participant_info) { + RMW_SET_ERROR_MSG("participant_info is null"); + return RMW_RET_ERROR; + } + + auto info = static_cast(subscription->data); + if (info != nullptr) { + if (info->subscriber_ != nullptr) { + Domain::removeSubscriber(info->subscriber_); + } + delete info->listener_; + if (info->type_support_ != nullptr) { + Participant * participant = participant_info->participant; + _unregister_type(participant, info->type_support_); + } + delete info; + } + rmw_free(const_cast(subscription->topic_name)); + subscription->topic_name = nullptr; + rmw_subscription_free(subscription); + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/test/CMakeLists.txt b/rmw_fastrtps_shared_cpp/test/CMakeLists.txt index 868a5bcbf..5cb62c731 100644 --- a/rmw_fastrtps_shared_cpp/test/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/test/CMakeLists.txt @@ -5,9 +5,3 @@ if(TARGET test_dds_attributes_to_rmw_qos) ament_target_dependencies(test_dds_attributes_to_rmw_qos) target_link_libraries(test_dds_attributes_to_rmw_qos ${PROJECT_NAME}) endif() - -ament_add_gtest(test_topic_cache test_topic_cache.cpp) -if(TARGET test_topic_cache) - ament_target_dependencies(test_topic_cache) - target_link_libraries(test_topic_cache ${PROJECT_NAME}) -endif() diff --git a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp deleted file mode 100644 index a93ba4de6..000000000 --- a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp +++ /dev/null @@ -1,262 +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 -#include -#include -#include - -#include "gtest/gtest.h" - -#include "rmw_fastrtps_shared_cpp/topic_cache.hpp" - -#include "fastrtps/rtps/common/InstanceHandle.h" -#include "fastrtps/qos/ReaderQos.h" -#include "fastrtps/qos/WriterQos.h" -#include "rmw/types.h" - -using eprosima::fastrtps::WriterQos; -using eprosima::fastrtps::ReaderQos; -using eprosima::fastrtps::rtps::GUID_t; -using eprosima::fastrtps::rtps::GuidPrefix_t; -using eprosima::fastrtps::rtps::InstanceHandle_t; - -class TopicCacheTestFixture : public ::testing::Test -{ -public: - TopicCache topic_cache; - WriterQos qos[2]; - rmw_qos_profile_t rmw_qos[2]; - InstanceHandle_t participant_instance_handler[2]; - GUID_t participant_guid[2]; - GUID_t guid[2]; - - void SetUp() - { - // Create instance handlers - for (int i = 0; i < 2; i++) { - guid[i] = GUID_t(GuidPrefix_t(), i + 100); - participant_guid[i] = GUID_t(GuidPrefix_t(), i + 1); - participant_instance_handler[i] = participant_guid[i]; - } - - // Populating WriterQos -> which is from the DDS layer and - // rmw_qos_profile_t which is from rmw/types.h. - // This is done to test if topic_cache.getTopicNameToTopicData() returns - // the correct value in rmw_qos_profile_t for a given WriterQos - - // DDS qos - qos[0].m_durability.kind = eprosima::fastrtps::TRANSIENT_DURABILITY_QOS; - qos[0].m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; - qos[0].m_deadline.period = {123, 5678}; - qos[0].m_lifespan.duration = {190, 1234}; - qos[0].m_liveliness.kind = eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS; - qos[0].m_liveliness.lease_duration = {80, 5555555}; - - // equivalent rmq qos - rmw_qos[0].durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - rmw_qos[0].reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - rmw_qos[0].liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; - rmw_qos[0].liveliness_lease_duration.sec = 80u; - rmw_qos[0].liveliness_lease_duration.nsec = 5555555u; - rmw_qos[0].deadline.sec = 123u; - rmw_qos[0].deadline.nsec = 5678u; - rmw_qos[0].lifespan.sec = 190u; - rmw_qos[0].lifespan.nsec = 1234u; - - // DDS qos - qos[1].m_durability.kind = eprosima::fastrtps::TRANSIENT_DURABILITY_QOS; - qos[1].m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; - qos[1].m_deadline.period = {12, 1234}; - qos[1].m_lifespan.duration = {19, 5432}; - qos[1].m_liveliness.kind = eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS; - qos[1].m_liveliness.lease_duration = {8, 78901234}; - - // equivalent rmq qos - rmw_qos[1].durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - rmw_qos[1].reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - rmw_qos[1].liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; - rmw_qos[1].liveliness_lease_duration.sec = 8u; - rmw_qos[1].liveliness_lease_duration.nsec = 78901234u; - rmw_qos[1].deadline.sec = 12u; - rmw_qos[1].deadline.nsec = 1234u; - rmw_qos[1].lifespan.sec = 19u; - rmw_qos[1].lifespan.nsec = 5432u; - - // Add data to topic_cache - topic_cache.addTopic(participant_instance_handler[0], guid[0], "topic1", "type1", qos[0]); - topic_cache.addTopic(participant_instance_handler[0], guid[0], "topic2", "type2", qos[0]); - topic_cache.addTopic(participant_instance_handler[1], guid[1], "topic1", "type1", qos[1]); - topic_cache.addTopic(participant_instance_handler[1], guid[1], "topic2", "type1", qos[1]); - } -}; - -TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_types) -{ - const auto topic_type_map = this->topic_cache.getTopicToTypes(); - - // topic1 - const auto & it = topic_type_map.find("topic1"); - ASSERT_TRUE(it != topic_type_map.end()); - // Verify that the object returned from the map is indeed the one expected - auto topic_types = it->second; - // Verify that there are two entries for type1 - EXPECT_EQ(std::count(topic_types.begin(), topic_types.end(), "type1"), 2); - EXPECT_EQ(topic_types.at(0), "type1"); - EXPECT_EQ(topic_types.at(1), "type1"); - - // topic2 - const auto & it2 = topic_type_map.find("topic2"); - ASSERT_TRUE(it2 != topic_type_map.end()); - // Verify that the object returned from the map is indeed the one expected - topic_types = it2->second; - // Verify that there are entries for type1 and type2 - EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type1") != topic_types.end()); - EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type2") != topic_types.end()); -} - -TEST_F(TopicCacheTestFixture, test_topic_cache_get_participant_map) -{ - const auto & participant_topic_map = this->topic_cache.getParticipantToTopics(); - - // participant 1 - const auto & it = participant_topic_map.find(this->participant_guid[0]); - ASSERT_TRUE(it != participant_topic_map.end()); - // Verify that the topic and respective types are present - const auto & topic_type_map = it->second; - - const auto & topic1it = topic_type_map.find("topic1"); - ASSERT_TRUE(topic1it != topic_type_map.end()); - auto topic_types = topic1it->second; - EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type1") != topic_types.end()); - - const auto & topic2it = topic_type_map.find("topic2"); - ASSERT_TRUE(topic2it != topic_type_map.end()); - topic_types = topic2it->second; - EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type2") != topic_types.end()); - - // participant 2 - const auto & it2 = participant_topic_map.find(this->participant_guid[1]); - ASSERT_TRUE(it2 != participant_topic_map.end()); - // Verify that the topic and respective types are present - const auto & topic_type_map2 = it2->second; - - const auto & topic1it2 = topic_type_map2.find("topic1"); - ASSERT_TRUE(topic1it2 != topic_type_map2.end()); - topic_types = topic1it2->second; - EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type1") != topic_types.end()); - - const auto & topic2it2 = topic_type_map2.find("topic2"); - ASSERT_TRUE(topic2it2 != topic_type_map2.end()); - topic_types = topic2it2->second; - EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type1") != topic_types.end()); -} - -TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_name_topic_data_map) -{ - const auto & topic_data_map = this->topic_cache.getTopicNameToTopicData(); - auto expected_results = std::map>(); - expected_results["topic1"].push_back({participant_guid[0], guid[0], "type1", rmw_qos[0]}); - expected_results["topic1"].push_back({participant_guid[1], guid[1], "type1", rmw_qos[1]}); - expected_results["topic2"].push_back({participant_guid[0], guid[0], "type2", rmw_qos[0]}); - expected_results["topic2"].push_back({participant_guid[1], guid[1], "type1", rmw_qos[1]}); - for (const auto & result_it : expected_results) { - const auto & topic_name = result_it.first; - const auto & expected_topic_data = result_it.second; - - const auto & it = topic_data_map.find(topic_name); - ASSERT_TRUE(it != topic_data_map.end()); - // Verify that the topic has all the associated data - const auto & topic_data = it->second; - for (auto i = 0u; i < expected_topic_data.size(); i++) { - // PARTICIPANT GUID - EXPECT_EQ(topic_data.at(i).participant_guid, expected_topic_data.at(i).participant_guid); - // GUID - EXPECT_EQ(topic_data.at(i).entity_guid, expected_topic_data.at(i).entity_guid); - // TYPE - EXPECT_EQ(topic_data.at(i).topic_type, expected_topic_data.at(i).topic_type); - // QOS - const auto & qos = topic_data.at(i).qos_profile; - const auto & expected_qos = expected_topic_data.at(i).qos_profile; - EXPECT_EQ(qos.durability, expected_qos.durability); - EXPECT_EQ(qos.reliability, expected_qos.reliability); - EXPECT_EQ(qos.liveliness, expected_qos.liveliness); - EXPECT_EQ(qos.liveliness_lease_duration.sec, expected_qos.liveliness_lease_duration.sec); - EXPECT_EQ(qos.liveliness_lease_duration.nsec, expected_qos.liveliness_lease_duration.nsec); - EXPECT_EQ(qos.deadline.sec, expected_qos.deadline.sec); - EXPECT_EQ(qos.deadline.nsec, expected_qos.deadline.nsec); - EXPECT_EQ(qos.lifespan.sec, expected_qos.lifespan.sec); - EXPECT_EQ(qos.lifespan.nsec, expected_qos.lifespan.nsec); - } - } -} - -TEST_F(TopicCacheTestFixture, test_topic_cache_add_topic) -{ - // Add Topic - const bool did_add = this->topic_cache.addTopic( - this->participant_instance_handler[1], this->guid[1], "TestTopic", "TestType", this->qos[1]); - // Verify that the returned value was true - EXPECT_TRUE(did_add); -} - -TEST_F(TopicCacheTestFixture, test_topic_cache_remove_topic_element_exists) -{ - auto did_remove = this->topic_cache.removeTopic( - this->participant_instance_handler[0], this->guid[0], "topic1", "type1"); - // Assert that the return was true - ASSERT_TRUE(did_remove); - // Verify it is removed from TopicToTypes - const auto & topic_type_map = this->topic_cache.getTopicToTypes(); - const auto & topic_type_it = topic_type_map.find("topic1"); - ASSERT_TRUE(topic_type_it != topic_type_map.end()); - EXPECT_EQ(std::count(topic_type_it->second.begin(), topic_type_it->second.end(), "type1"), 1); - // Verify it is removed from ParticipantTopicMap - const auto & participant_topic_map = this->topic_cache.getParticipantToTopics(); - const auto & participant_topic_it = participant_topic_map.find(this->participant_guid[0]); - ASSERT_TRUE(participant_topic_it != participant_topic_map.end()); - const auto & p_topic_type_it = participant_topic_it->second.find("topic1"); - ASSERT_TRUE(p_topic_type_it == participant_topic_it->second.end()); - // Verify it is removed from TopicNameToTopicTypeMap - const auto & topic_data_map = this->topic_cache.getTopicNameToTopicData(); - const auto & topic_data_it = topic_data_map.find("topic1"); - ASSERT_TRUE(topic_data_it != topic_data_map.end()); - EXPECT_EQ(topic_data_it->second.size(), 1u); - - did_remove = this->topic_cache.removeTopic( - this->participant_instance_handler[1], this->guid[1], "topic1", "type1"); - ASSERT_TRUE(did_remove); - const auto & topic_type_map2 = this->topic_cache.getTopicToTypes(); - const auto & topic_type_it2 = topic_type_map2.find("topic1"); - ASSERT_TRUE(topic_type_it2 == topic_type_map2.end()); - const auto & participant_topic_map2 = this->topic_cache.getParticipantToTopics(); - const auto & participant_topic_it2 = participant_topic_map2.find(this->participant_guid[1]); - ASSERT_TRUE(participant_topic_it2 != participant_topic_map2.end()); - const auto & p_topic_type_it2 = participant_topic_it2->second.find("topic1"); - ASSERT_TRUE(p_topic_type_it2 == participant_topic_it2->second.end()); - const auto & topic_data_map2 = this->topic_cache.getTopicNameToTopicData(); - const auto & topic_data_it2 = topic_data_map2.find("topic1"); - ASSERT_TRUE(topic_data_it2 == topic_data_map2.end()); -} - -TEST_F(TopicCacheTestFixture, test_topic_cache_remove_policy_element_does_not_exist) -{ - // add topic - this->topic_cache.addTopic( - this->participant_instance_handler[1], this->guid[1], "TestTopic", "TestType", this->qos[1]); - // Assert that the return was false - const auto did_remove = this->topic_cache.removeTopic( - this->participant_instance_handler[1], this->guid[1], "NewTestTopic", "TestType"); - ASSERT_FALSE(did_remove); -} From 826d388da28c79d7a4abe120cc8c4ce13bab8cd0 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 4 Feb 2020 16:12:50 -0300 Subject: [PATCH 02/37] Fix error, use mangled topic name Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp index f8bc7b32e..afff7af16 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -106,7 +106,7 @@ __rmw_get_publishers_info_by_topic( } return graph_cache.get_writers_info_by_topic( - topic_name, + mangled_topic_name, demangle_type, allocator, publishers_info); @@ -140,7 +140,7 @@ __rmw_get_subscriptions_info_by_topic( } return graph_cache.get_readers_info_by_topic( - topic_name, + mangled_topic_name, demangle_type, allocator, subscriptions_info); From 76123eff68615062037334a3f97fcf75605f95c0 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 5 Feb 2020 15:56:47 -0300 Subject: [PATCH 03/37] Address peer review comments Signed-off-by: Ivan Santiago Paunovic --- .../rmw_fastrtps_cpp/register_node.hpp | 10 ++-- rmw_fastrtps_cpp/src/listener_thread.cpp | 9 +-- rmw_fastrtps_cpp/src/register_node.cpp | 55 +++++++++++++------ rmw_fastrtps_cpp/src/rmw_node.cpp | 15 ++++- .../register_node.hpp | 10 ++-- .../src/listener_thread.cpp | 9 +-- .../src/register_node.cpp | 55 +++++++++++++------ rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp | 17 ++++-- .../include/rmw_fastrtps_shared_cpp/names.hpp | 24 ++++++-- .../rmw_context_impl.hpp | 4 ++ rmw_fastrtps_shared_cpp/src/demangle.cpp | 4 +- rmw_fastrtps_shared_cpp/src/participant.cpp | 2 - rmw_fastrtps_shared_cpp/src/qos.cpp | 1 - 13 files changed, 138 insertions(+), 77 deletions(-) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/register_node.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/register_node.hpp index 2ab12b6b0..2ef2d312e 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/register_node.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/register_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. +// 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. @@ -21,20 +21,20 @@ namespace rmw_fastrtps_cpp { -/// Register node in context. +/// Increment `rmw_context_impl_t` reference count, initing it necessary. /** * Function that should be called when creating a node, * before using `context->impl`. */ rmw_ret_t -register_node(rmw_context_t * context); +increment_context_impl_ref_count(rmw_context_t * context); -/// Unregister node in context. +/// Increment `rmw_context_impl_t` reference count, destroying it if the count reaches zero. /** * Function that should be called when destroying a node. */ rmw_ret_t -unregister_node(rmw_context_t * context); +decrement_context_impl_ref_count(rmw_context_t * context); } // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/src/listener_thread.cpp b/rmw_fastrtps_cpp/src/listener_thread.cpp index 590ed07ed..228307eb0 100644 --- a/rmw_fastrtps_cpp/src/listener_thread.cpp +++ b/rmw_fastrtps_cpp/src/listener_thread.cpp @@ -62,8 +62,7 @@ rmw_fastrtps_cpp::run_listener_thread(rmw_context_t * context) common_context->thread_is_running.store(false); if (common_context->listener_thread_gc) { if (RMW_RET_OK != rmw_destroy_guard_condition(common_context->listener_thread_gc)) { - fprintf( - stderr, + RCUTILS_SAFE_FWRITE_TO_STDERR( RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); } @@ -98,11 +97,9 @@ rmw_fastrtps_cpp::join_listener_thread(rmw_context_t * context) #define TERMINATE(msg) \ do { \ - fprintf( \ - stderr, \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ - RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) ": %s, terminating ...", \ - rmw_get_error_string().str); \ + RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) ": terminating ..."); \ std::terminate(); \ } while (0) diff --git a/rmw_fastrtps_cpp/src/register_node.cpp b/rmw_fastrtps_cpp/src/register_node.cpp index 89a22b096..d969a9d9b 100644 --- a/rmw_fastrtps_cpp/src/register_node.cpp +++ b/rmw_fastrtps_cpp/src/register_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. +// 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. @@ -66,8 +66,8 @@ init_context_impl(rmw_context_t * context) common_context.get()), [&](CustomParticipantInfo * participant_info) { if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { - fprintf( - stderr, "Failed to destroy participant after function: '" + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy participant after function: '" RCUTILS_STRINGIFY(__function__) "' failed.\n"); } }); @@ -99,8 +99,8 @@ init_context_impl(rmw_context_t * context) participant_info.get(), pub)) { - fprintf( - stderr, "Failed to destroy publisher after function: '" + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy publisher after function: '" RCUTILS_STRINGIFY(__function__) "' failed.\n"); } }); @@ -143,8 +143,9 @@ init_context_impl(rmw_context_t * context) context->impl->common = common_context.get(); context->impl->participant_info = participant_info.get(); - if (RMW_RET_OK != rmw_fastrtps_cpp::run_listener_thread(context)) { - return RMW_RET_ERROR; + rmw_ret_t ret = rmw_fastrtps_cpp::run_listener_thread(context); + if (RMW_RET_OK != ret) { + return ret; } common_context->graph_cache.add_participant(common_context->gid); @@ -156,7 +157,7 @@ init_context_impl(rmw_context_t * context) } rmw_ret_t -rmw_fastrtps_cpp::register_node(rmw_context_t * context) +rmw_fastrtps_cpp::increment_context_impl_ref_count(rmw_context_t * context) { assert(context); assert(context->impl); @@ -174,14 +175,19 @@ rmw_fastrtps_cpp::register_node(rmw_context_t * context) } rmw_ret_t -rmw_fastrtps_cpp::unregister_node(rmw_context_t * context) +rmw_fastrtps_cpp::decrement_context_impl_ref_count(rmw_context_t * context) { + std::lock_guard guard(context->impl->mutex); + assert(context); assert(context->impl); + assert(0u < context->impl->count); - std::lock_guard guard(context->impl->mutex); if (0u == --context->impl->count) { - rmw_ret_t ret; + rmw_ret_t err = RMW_RET_OK; + rmw_ret_t ret = RMW_RET_OK; + rmw_error_string_t error_string; + ret = rmw_fastrtps_cpp::join_listener_thread(context); if (RMW_RET_OK != ret) { return ret; @@ -195,19 +201,32 @@ rmw_fastrtps_cpp::unregister_node(rmw_context_t * context) eprosima_fastrtps_identifier, participant_info, common_context->sub); + // Try to clean the other objects if the above failed. if (RMW_RET_OK != ret) { - return ret; + error_string = rmw_get_error_string(); + rmw_reset_error(); } - ret = rmw_fastrtps_shared_cpp::destroy_publisher( + err = rmw_fastrtps_shared_cpp::destroy_publisher( eprosima_fastrtps_identifier, participant_info, common_context->pub); - if (RMW_RET_OK != ret) { - return ret; + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + // We can just return one error, log about the previous one. + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_subscription' failed\n"); + ret = err; + error_string = rmw_get_error_string(); + rmw_reset_error(); } - ret = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); - if (RMW_RET_OK != ret) { - return ret; + err = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_publisher' failed\n"); + ret = err; + } else if (RMW_RET_OK != ret) { + RMW_SET_ERROR_MSG(error_string.str); } delete common_context; context->impl->common = nullptr; diff --git a/rmw_fastrtps_cpp/src/rmw_node.cpp b/rmw_fastrtps_cpp/src/rmw_node.cpp index be8dc1236..1a2d42103 100644 --- a/rmw_fastrtps_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node.cpp @@ -54,12 +54,21 @@ rmw_create_node( // TODO(wjwwood): replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when refactored return nullptr); - if (RMW_RET_OK != rmw_fastrtps_cpp::register_node(context)) { + if (RMW_RET_OK != rmw_fastrtps_cpp::increment_context_impl_ref_count(context)) { return nullptr; } - return rmw_fastrtps_shared_cpp::__rmw_create_node( + rmw_node_t * node = rmw_fastrtps_shared_cpp::__rmw_create_node( context, eprosima_fastrtps_identifier, name, namespace_); + + if (nullptr == node) { + if (RMW_RET_OK != rmw_fastrtps_cpp::decrement_context_impl_ref_count(context)) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "'decrement_context_impl_ref_count' failed while being executed due to '" + RCUTILS_STRINGIFY(__function__) "' failing"); + } + } + return node; } rmw_ret_t @@ -70,7 +79,7 @@ rmw_destroy_node(rmw_node_t * node) if (RMW_RET_OK != ret) { return ret; } - return rmw_fastrtps_cpp::unregister_node(node->context); + return rmw_fastrtps_cpp::decrement_context_impl_ref_count(node->context); } rmw_ret_t diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/register_node.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/register_node.hpp index 5abde1349..9b634b48a 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/register_node.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/register_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. +// 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. @@ -21,20 +21,20 @@ namespace rmw_fastrtps_dynamic_cpp { -/// Register node in context. +/// Increment `rmw_context_impl_t` reference count, initing it necessary. /** * Function that should be called when creating a node, * before using `context->impl`. */ rmw_ret_t -register_node(rmw_context_t * context); +increment_context_impl_ref_count(rmw_context_t * context); -/// Unregister node in context. +/// Increment `rmw_context_impl_t` reference count, destroying it if the count reaches zero. /** * Function that should be called when destroying a node. */ rmw_ret_t -unregister_node(rmw_context_t * context); +decrement_context_impl_ref_count(rmw_context_t * context); } // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp b/rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp index 34c3208a3..1bbfac93c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp @@ -62,8 +62,7 @@ rmw_fastrtps_dynamic_cpp::run_listener_thread(rmw_context_t * context) common_context->thread_is_running.store(false); if (common_context->listener_thread_gc) { if (RMW_RET_OK != rmw_destroy_guard_condition(common_context->listener_thread_gc)) { - fprintf( - stderr, + RCUTILS_SAFE_FWRITE_TO_STDERR( RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); } @@ -98,11 +97,9 @@ rmw_fastrtps_dynamic_cpp::join_listener_thread(rmw_context_t * context) #define TERMINATE(msg) \ do { \ - fprintf( \ - stderr, \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ - RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) ": %s, terminating ...", \ - rmw_get_error_string().str); \ + RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) ": terminating ..."); \ std::terminate(); \ } while (0) diff --git a/rmw_fastrtps_dynamic_cpp/src/register_node.cpp b/rmw_fastrtps_dynamic_cpp/src/register_node.cpp index 6ab88330a..082b36ca7 100644 --- a/rmw_fastrtps_dynamic_cpp/src/register_node.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/register_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. +// 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. @@ -66,8 +66,8 @@ init_context_impl(rmw_context_t * context) common_context.get()), [&](CustomParticipantInfo * participant_info) { if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { - fprintf( - stderr, "Failed to destroy participant after function: '" + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy participant after function: '" RCUTILS_STRINGIFY(__function__) "' failed.\n"); } }); @@ -99,8 +99,8 @@ init_context_impl(rmw_context_t * context) participant_info.get(), pub)) { - fprintf( - stderr, "Failed to destroy publisher after function: '" + RCUTILS_SAFE_FWRITE_TO_STDERR( + "Failed to destroy publisher after function: '" RCUTILS_STRINGIFY(__function__) "' failed.\n"); } }); @@ -143,8 +143,9 @@ init_context_impl(rmw_context_t * context) context->impl->common = common_context.get(); context->impl->participant_info = participant_info.get(); - if (RMW_RET_OK != rmw_fastrtps_dynamic_cpp::run_listener_thread(context)) { - return RMW_RET_ERROR; + rmw_ret_t ret = rmw_fastrtps_dynamic_cpp::run_listener_thread(context); + if (RMW_RET_OK != ret) { + return ret; } common_context->graph_cache.add_participant(common_context->gid); @@ -156,7 +157,7 @@ init_context_impl(rmw_context_t * context) } rmw_ret_t -rmw_fastrtps_dynamic_cpp::register_node(rmw_context_t * context) +rmw_fastrtps_dynamic_cpp::increment_context_impl_ref_count(rmw_context_t * context) { assert(context); assert(context->impl); @@ -174,14 +175,19 @@ rmw_fastrtps_dynamic_cpp::register_node(rmw_context_t * context) } rmw_ret_t -rmw_fastrtps_dynamic_cpp::unregister_node(rmw_context_t * context) +rmw_fastrtps_dynamic_cpp::decrement_context_impl_ref_count(rmw_context_t * context) { + std::lock_guard guard(context->impl->mutex); + assert(context); assert(context->impl); + assert(0u < context->impl->count); - std::lock_guard guard(context->impl->mutex); if (0u == --context->impl->count) { - rmw_ret_t ret; + rmw_ret_t err = RMW_RET_OK; + rmw_ret_t ret = RMW_RET_OK; + rmw_error_string_t error_string; + ret = rmw_fastrtps_dynamic_cpp::join_listener_thread(context); if (RMW_RET_OK != ret) { return ret; @@ -195,19 +201,32 @@ rmw_fastrtps_dynamic_cpp::unregister_node(rmw_context_t * context) eprosima_fastrtps_identifier, participant_info, common_context->sub); + // Try to clean the other objects if the above failed. if (RMW_RET_OK != ret) { - return ret; + error_string = rmw_get_error_string(); + rmw_reset_error(); } - ret = rmw_fastrtps_shared_cpp::destroy_publisher( + err = rmw_fastrtps_shared_cpp::destroy_publisher( eprosima_fastrtps_identifier, participant_info, common_context->pub); - if (RMW_RET_OK != ret) { - return ret; + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + // We can just return one error, log about the previous one. + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_subscription' failed\n"); + ret = err; + error_string = rmw_get_error_string(); + rmw_reset_error(); } - ret = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); - if (RMW_RET_OK != ret) { - return ret; + err = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_publisher' failed\n"); + ret = err; + } else if (RMW_RET_OK != ret) { + RMW_SET_ERROR_MSG(error_string.str); } delete common_context; context->impl->common = nullptr; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp index 7fe7fb4f4..66be4d91e 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp @@ -52,12 +52,17 @@ rmw_create_node( // TODO(wjwwood): replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when refactored return NULL); - if (RMW_RET_OK != rmw_fastrtps_dynamic_cpp::register_node(context)) { - return nullptr; - } - - return rmw_fastrtps_shared_cpp::__rmw_create_node( + rmw_node_t * node = rmw_fastrtps_shared_cpp::__rmw_create_node( context, eprosima_fastrtps_identifier, name, namespace_); + + if (nullptr == node) { + if (RMW_RET_OK != rmw_fastrtps_dynamic_cpp::decrement_context_impl_ref_count(context)) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "'decrement_context_impl_ref_count' failed while being executed due to '" + RCUTILS_STRINGIFY(__function__) "' failing"); + } + } + return node; } rmw_ret_t @@ -68,7 +73,7 @@ rmw_destroy_node(rmw_node_t * node) if (RMW_RET_OK != ret) { return ret; } - return rmw_fastrtps_dynamic_cpp::unregister_node(node->context); + return rmw_fastrtps_dynamic_cpp::decrement_context_impl_ref_count(node->context); } rmw_ret_t diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp index 423286d19..6af152312 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp @@ -28,8 +28,8 @@ * \param[in] suffix Optional suffix for topic name. */ inline -std::string -_mangle_topic_name( +eprosima::fastrtps::string_255 +_mangle_topic_name_impl( const char * prefix, const char * base, const char * suffix = nullptr) @@ -45,6 +45,22 @@ _mangle_topic_name( return topicName.str(); } +/// Construct a topic name. +/** + * \param[in] prefix Required prefix for topic name. + * \param[in] base Required name of the topic. + * \param[in] suffix Optional suffix for topic name. + */ +inline +std::string +_mangle_topic_name( + const char * prefix, + const char * base, + const char * suffix = nullptr) +{ + return _mangle_topic_name_impl(prefix, base, suffix).to_string(); +} + /// Construct a topic name according to proper conventions. /** * \param[in] qos_profile The QoS profile for the topic. @@ -53,7 +69,7 @@ _mangle_topic_name( * \param[in] suffix Optional suffix for topic name. */ inline -std::string +eprosima::fastrtps::string_255 _create_topic_name( const rmw_qos_profile_t * qos_profile, const char * prefix, @@ -63,7 +79,7 @@ _create_topic_name( if (qos_profile->avoid_ros_namespace_conventions) { prefix = nullptr; } - return _mangle_topic_name(prefix, base, suffix); + return _mangle_topic_name_impl(prefix, base, suffix); } #endif // RMW_FASTRTPS_SHARED_CPP__NAMES_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_context_impl.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_context_impl.hpp index 6a985561a..9132363b7 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_context_impl.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_context_impl.hpp @@ -19,9 +19,13 @@ struct rmw_context_impl_t { + /// Pointer to `rmw_dds_common::Context`. void * common; + /// Pointer to `rmw_fastrtps_shared_cpp::CustomParticipantInfo`. void * participant_info; + /// Mutex used to protect initialization/destruction. std::mutex mutex; + /// Reference count. uint64_t count; }; diff --git a/rmw_fastrtps_shared_cpp/src/demangle.cpp b/rmw_fastrtps_shared_cpp/src/demangle.cpp index fd9103f70..15e4e3f2d 100644 --- a/rmw_fastrtps_shared_cpp/src/demangle.cpp +++ b/rmw_fastrtps_shared_cpp/src/demangle.cpp @@ -81,9 +81,7 @@ _demangle_service_from_topic( ", report this: '%s'", topic_name.c_str()); return ""; } - } - - if (std::string::npos == suffix_position) { + } else { RCUTILS_LOG_WARN_NAMED( "rmw_fastrtps_shared_cpp", "service topic has prefix but no suffix" diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 12ede8dae..c3618fc7c 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -112,8 +112,6 @@ __create_participant( return nullptr; } -#define ROS_DOMAIN_ID_VAR_NAME "ROS_DOMAIN_ID" - CustomParticipantInfo * rmw_fastrtps_shared_cpp::create_participant( const char * identifier, diff --git a/rmw_fastrtps_shared_cpp/src/qos.cpp b/rmw_fastrtps_shared_cpp/src/qos.cpp index 40645e5ec..e6ca5a20c 100644 --- a/rmw_fastrtps_shared_cpp/src/qos.cpp +++ b/rmw_fastrtps_shared_cpp/src/qos.cpp @@ -1,4 +1,3 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); From 4e9c9f0a990f8df61605de3ddd845ad70cb8f9fc Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 5 Feb 2020 16:01:44 -0300 Subject: [PATCH 04/37] Rename register_node.* Signed-off-by: Ivan Santiago Paunovic --- .../{register_node.hpp => init_rmw_context_impl.hpp} | 0 .../src/{register_node.cpp => init_rmw_context_impl.cpp} | 0 .../{register_node.hpp => init_rmw_context_impl.hpp} | 0 .../src/{register_node.cpp => init_rmw_context_impl.cpp} | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/{register_node.hpp => init_rmw_context_impl.hpp} (100%) rename rmw_fastrtps_cpp/src/{register_node.cpp => init_rmw_context_impl.cpp} (100%) rename rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/{register_node.hpp => init_rmw_context_impl.hpp} (100%) rename rmw_fastrtps_dynamic_cpp/src/{register_node.cpp => init_rmw_context_impl.cpp} (100%) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/register_node.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp similarity index 100% rename from rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/register_node.hpp rename to rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp diff --git a/rmw_fastrtps_cpp/src/register_node.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp similarity index 100% rename from rmw_fastrtps_cpp/src/register_node.cpp rename to rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/register_node.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp similarity index 100% rename from rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/register_node.hpp rename to rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp diff --git a/rmw_fastrtps_dynamic_cpp/src/register_node.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp similarity index 100% rename from rmw_fastrtps_dynamic_cpp/src/register_node.cpp rename to rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp From be9d13fad5354263fab23f484ee14570b4ac7555 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 5 Feb 2020 16:18:15 -0300 Subject: [PATCH 05/37] Updates after renaming register_node.* Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/CMakeLists.txt | 2 +- .../include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp | 6 +++--- rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp | 2 +- rmw_fastrtps_cpp/src/rmw_node.cpp | 2 +- rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 2 +- .../rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp | 6 +++--- rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp | 2 +- 8 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 41ff3b908..df1980b19 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -54,9 +54,9 @@ add_library(rmw_fastrtps_cpp src/get_service.cpp src/get_subscriber.cpp src/identifier.cpp + src/init_rmw_context_impl.cpp src/listener_thread.cpp src/publisher.cpp - src/register_node.cpp src/rmw_logging.cpp src/rmw_client.cpp src/rmw_compare_gids_equal.cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp index 2ef2d312e..7b9248999 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_FASTRTPS_CPP__REGISTER_NODE_HPP_ -#define RMW_FASTRTPS_CPP__REGISTER_NODE_HPP_ +#ifndef RMW_FASTRTPS_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ +#define RMW_FASTRTPS_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ #include "rmw/init.h" #include "rmw/types.h" @@ -38,4 +38,4 @@ decrement_context_impl_ref_count(rmw_context_t * context); } // namespace rmw_fastrtps_cpp -#endif // RMW_FASTRTPS_CPP__REGISTER_NODE_HPP_ +#endif // RMW_FASTRTPS_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index d969a9d9b..1fed26fe7 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rmw_fastrtps_cpp/register_node.hpp" +#include "rmw_fastrtps_cpp/init_rmw_context_impl.hpp" #include #include diff --git a/rmw_fastrtps_cpp/src/rmw_node.cpp b/rmw_fastrtps_cpp/src/rmw_node.cpp index 1a2d42103..f6d4bfa6e 100644 --- a/rmw_fastrtps_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node.cpp @@ -30,7 +30,7 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" -#include "rmw_fastrtps_cpp/register_node.hpp" +#include "rmw_fastrtps_cpp/init_rmw_context_impl.hpp" extern "C" { diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index 1cab6e08b..ffdc03fab 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -59,11 +59,11 @@ add_library(rmw_fastrtps_dynamic_cpp src/get_subscriber.cpp src/gid__type_support.cpp src/identifier.cpp + src/init_rmw_context_impl.cpp src/listener_thread.cpp src/node_entities_info__type_support.cpp src/participant_entities_info__type_support.cpp src/publisher.cpp - src/register_node.cpp src/rmw_client.cpp src/rmw_compare_gids_equal.cpp src/rmw_count.cpp diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp index 9b634b48a..eeeafcd65 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RMW_FASTRTPS_DYNAMIC_CPP__REGISTER_NODE_HPP_ -#define RMW_FASTRTPS_DYNAMIC_CPP__REGISTER_NODE_HPP_ +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ #include "rmw/init.h" #include "rmw/types.h" @@ -38,4 +38,4 @@ decrement_context_impl_ref_count(rmw_context_t * context); } // namespace rmw_fastrtps_dynamic_cpp -#endif // RMW_FASTRTPS_DYNAMIC_CPP__REGISTER_NODE_HPP_ +#endif // RMW_FASTRTPS_DYNAMIC_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index 082b36ca7..df6a768d6 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rmw_fastrtps_dynamic_cpp/register_node.hpp" +#include "rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp" #include #include diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp index 66be4d91e..6dea56047 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp @@ -28,7 +28,7 @@ #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" -#include "rmw_fastrtps_dynamic_cpp/register_node.hpp" +#include "rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp" extern "C" { From 0659c316ce50648316bc819ec7b964f7cfec1f59 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 5 Feb 2020 16:18:54 -0300 Subject: [PATCH 06/37] Update rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp to match rmw_fastrtps_cpp implementation Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/rmw_init.cpp | 8 ++-- rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp | 49 ++++++++++++++++++++--- 2 files changed, 47 insertions(+), 10 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_init.cpp b/rmw_fastrtps_cpp/src/rmw_init.cpp index 36ab04176..d04956b04 100644 --- a/rmw_fastrtps_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_cpp/src/rmw_init.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -159,9 +159,9 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) ret = rmw_init_options_copy(options, &context->options); if (RMW_RET_OK != ret) { if (RMW_RET_OK != rmw_init_options_fini(&context->options)) { - fprintf( - stderr, - "Failed to destroy init options after ':" RCUTILS_STRINGIFY(__function__) "' failed.\n"); + RMW_SAFE_FWRITE_TO_STDERR( + "'rmw_init_options_fini' failed while being executed due to '" + RCUTILS_STRINGIFY(__function__) "' failing.\n"); } return ret; } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp index 7a979829a..df59d96e2 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -15,6 +15,7 @@ #include +#include "rcutils/strdup.h" #include "rcutils/types.h" #include "rmw/impl/cpp/macros.hpp" @@ -55,6 +56,17 @@ rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t all init_options->implementation_identifier = eprosima_fastrtps_identifier; init_options->allocator = allocator; init_options->impl = nullptr; + init_options->name = rcutils_strdup("", allocator); + if (!init_options->name) { + RMW_SET_ERROR_MSG("failed to copy context name"); + return RMW_RET_BAD_ALLOC; + } + init_options->namespace_ = rcutils_strdup("", allocator); + if (!init_options->namespace_) { + allocator.deallocate(init_options->name, allocator.state); + RMW_SET_ERROR_MSG("failed to copy context namespace"); + return RMW_RET_BAD_ALLOC; + } return RMW_RET_OK; } @@ -72,20 +84,45 @@ rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) RMW_SET_ERROR_MSG("expected zero-initialized dst"); return RMW_RET_INVALID_ARGUMENT; } + const rcutils_allocator_t * allocator = &src->allocator; + rmw_ret_t ret = RMW_RET_OK; + *dst = *src; - return RMW_RET_OK; + dst->name = NULL; + dst->namespace_ = NULL; + dst->security_options = rmw_get_zero_initialized_security_options(); + + dst->name = rcutils_strdup(src->name, *allocator); + if (!dst->name) { + ret = RMW_RET_BAD_ALLOC; + goto fail; + } + dst->namespace_ = rcutils_strdup(src->namespace_, *allocator); + if (!dst->namespace_) { + ret = RMW_RET_BAD_ALLOC; + goto fail; + } + return rmw_security_options_copy(&src->security_options, allocator, &dst->security_options); +fail: + allocator->deallocate(dst->name, allocator->state); + allocator->deallocate(dst->namespace_, allocator->state); + return ret; } rmw_ret_t rmw_init_options_fini(rmw_init_options_t * init_options) { RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR(&(init_options->allocator), return RMW_RET_INVALID_ARGUMENT); + rcutils_allocator_t & allocator = init_options->allocator; + RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( init_options, init_options->implementation_identifier, eprosima_fastrtps_identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + allocator.deallocate(init_options->name, allocator.state); + allocator.deallocate(init_options->namespace_, allocator.state); + rmw_security_options_fini(&init_options->security_options, &allocator); *init_options = rmw_get_zero_initialized_init_options(); return RMW_RET_OK; } @@ -121,9 +158,9 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) ret = rmw_init_options_copy(options, &context->options); if (RMW_RET_OK != ret) { if (RMW_RET_OK != rmw_init_options_fini(&context->options)) { - fprintf( - stderr, - "Failed to destroy init options after ':" RCUTILS_STRINGIFY(__function__) "' failed.\n"); + RMW_SAFE_FWRITE_TO_STDERR( + "'rmw_init_options_fini' failed while being executed due to '" + RCUTILS_STRINGIFY(__function__) "' failing.\n"); } return ret; } From df0f6ca2cc0604f04a11d8fa12e850a141f66173 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 5 Feb 2020 16:19:43 -0300 Subject: [PATCH 07/37] Get rid of fprintf usage Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index 1fed26fe7..ecedf09c8 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -126,8 +126,8 @@ init_context_impl(rmw_context_t * context) participant_info.get(), sub)) { - fprintf( - stderr, "Failed to destroy subscription after function: '" + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy subscription after function: '" RCUTILS_STRINGIFY(__function__) "' failed.\n"); } }); diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index df6a768d6..72bf4998b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -126,8 +126,8 @@ init_context_impl(rmw_context_t * context) participant_info.get(), sub)) { - fprintf( - stderr, "Failed to destroy subscription after function: '" + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy subscription after function: '" RCUTILS_STRINGIFY(__function__) "' failed.\n"); } }); From 554f2a405f9148d979f7c052752936173633c694 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 5 Feb 2020 17:09:40 -0300 Subject: [PATCH 08/37] Moved listener_thread.* to rmw_fastrpts_shared_cpp Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/CMakeLists.txt | 1 - .../src/init_rmw_context_impl.cpp | 6 +- rmw_fastrtps_cpp/src/listener_thread.cpp | 167 ------------------ rmw_fastrtps_cpp/src/rmw_init.cpp | 2 - rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 1 - .../src/init_rmw_context_impl.cpp | 6 +- .../src/listener_thread.hpp | 30 ---- rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp | 2 - rmw_fastrtps_shared_cpp/CMakeLists.txt | 1 + .../listener_thread.hpp | 12 +- .../src/listener_thread.cpp | 37 ++-- 11 files changed, 36 insertions(+), 229 deletions(-) delete mode 100644 rmw_fastrtps_cpp/src/listener_thread.cpp delete mode 100644 rmw_fastrtps_dynamic_cpp/src/listener_thread.hpp rename {rmw_fastrtps_cpp/src => rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp}/listener_thread.hpp (70%) rename {rmw_fastrtps_dynamic_cpp => rmw_fastrtps_shared_cpp}/src/listener_thread.cpp (78%) diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index df1980b19..9fed15b68 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -55,7 +55,6 @@ add_library(rmw_fastrtps_cpp src/get_subscriber.cpp src/identifier.cpp src/init_rmw_context_impl.cpp - src/listener_thread.cpp src/publisher.cpp src/rmw_logging.cpp src/rmw_client.cpp diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index ecedf09c8..34e49d329 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -36,7 +36,7 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" -#include "listener_thread.hpp" +#include "rmw_fastrtps_shared_cpp/listener_thread.hpp" using rmw_dds_common::msg::ParticipantEntitiesInfo; @@ -143,7 +143,7 @@ init_context_impl(rmw_context_t * context) context->impl->common = common_context.get(); context->impl->participant_info = participant_info.get(); - rmw_ret_t ret = rmw_fastrtps_cpp::run_listener_thread(context); + rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); if (RMW_RET_OK != ret) { return ret; } @@ -188,7 +188,7 @@ rmw_fastrtps_cpp::decrement_context_impl_ref_count(rmw_context_t * context) rmw_ret_t ret = RMW_RET_OK; rmw_error_string_t error_string; - ret = rmw_fastrtps_cpp::join_listener_thread(context); + ret = rmw_fastrtps_shared_cpp::join_listener_thread(context); if (RMW_RET_OK != ret) { return ret; } diff --git a/rmw_fastrtps_cpp/src/listener_thread.cpp b/rmw_fastrtps_cpp/src/listener_thread.cpp deleted file mode 100644 index 228307eb0..000000000 --- a/rmw_fastrtps_cpp/src/listener_thread.cpp +++ /dev/null @@ -1,167 +0,0 @@ -// 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 "rcutils/macros.h" - -#include "rmw/allocators.h" -#include "rmw/error_handling.h" -#include "rmw/init.h" -#include "rmw/ret_types.h" -#include "rmw/rmw.h" -#include "rmw/types.h" -#include "rmw/impl/cpp/macros.hpp" - -#include "rmw_dds_common/context.hpp" -#include "rmw_dds_common/gid_utils.hpp" -#include "rmw_dds_common/msg/participant_entities_info.hpp" - -#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" - -#include "listener_thread.hpp" - -using rmw_dds_common::operator<<; - -static -void -node_listener(rmw_context_t * context); - -rmw_ret_t -rmw_fastrtps_cpp::run_listener_thread(rmw_context_t * context) -{ - auto common_context = static_cast(context->impl->common); - common_context->thread_is_running.store(true); - common_context->listener_thread_gc = rmw_create_guard_condition(context); - if (common_context->listener_thread_gc) { - try { - common_context->listener_thread = std::thread(node_listener, context); - return RMW_RET_OK; - } catch (const std::exception & exc) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to create std::thread: %s", exc.what()); - } catch (...) { - RMW_SET_ERROR_MSG("Failed to create std::thread"); - } - } else { - RMW_SET_ERROR_MSG("Failed to create guard condition"); - } - common_context->thread_is_running.store(false); - if (common_context->listener_thread_gc) { - if (RMW_RET_OK != rmw_destroy_guard_condition(common_context->listener_thread_gc)) { - RCUTILS_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" - RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); - } - } - return RMW_RET_ERROR; -} - -rmw_ret_t -rmw_fastrtps_cpp::join_listener_thread(rmw_context_t * context) -{ - auto common_context = static_cast(context->impl->common); - common_context->thread_is_running.exchange(false); - rmw_ret_t rmw_ret = rmw_trigger_guard_condition(common_context->listener_thread_gc); - if (RMW_RET_OK != rmw_ret) { - return rmw_ret; - } - try { - common_context->listener_thread.join(); - } catch (const std::exception & exc) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to join std::thread: %s", exc.what()); - return RMW_RET_ERROR; - } catch (...) { - RMW_SET_ERROR_MSG("Failed to join std::thread"); - return RMW_RET_ERROR; - } - rmw_ret = rmw_destroy_guard_condition(common_context->listener_thread_gc); - if (RMW_RET_OK != rmw_ret) { - return rmw_ret; - } - return RMW_RET_OK; -} - -#define TERMINATE(msg) \ - do { \ - RCUTILS_SAFE_FWRITE_TO_STDERR( \ - RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ - RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) ": terminating ..."); \ - std::terminate(); \ - } while (0) - -void -node_listener(rmw_context_t * context) -{ - assert(nullptr != context); - assert(nullptr != context->impl); - assert(nullptr != context->impl->common); - auto common_context = static_cast(context->impl->common); - while (common_context->thread_is_running.load()) { - assert(nullptr != common_context->sub); - assert(nullptr != common_context->sub->data); - void * subscriptions_buffer[] = {common_context->sub->data}; - void * guard_conditions_buffer[] = {common_context->listener_thread_gc->data}; - rmw_subscriptions_t subscriptions; - rmw_guard_conditions_t guard_conditions; - subscriptions.subscriber_count = 1; - subscriptions.subscribers = subscriptions_buffer; - guard_conditions.guard_condition_count = 1; - guard_conditions.guard_conditions = guard_conditions_buffer; - // number of conditions of a subscription is 2 - rmw_wait_set_t * wait_set = rmw_create_wait_set(context, 2); - if (nullptr == wait_set) { - TERMINATE("failed to create wait set"); - } - if (RMW_RET_OK != rmw_wait( - &subscriptions, - &guard_conditions, - nullptr, - nullptr, - nullptr, - wait_set, - nullptr)) - { - TERMINATE("rmw_wait failed"); - } - if (subscriptions_buffer[0]) { - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - bool taken; - if (RMW_RET_OK != rmw_take( - common_context->sub, - static_cast(&msg), - &taken, - nullptr)) - { - TERMINATE("rmw_take failed"); - } - if (taken) { - if (std::memcmp( - reinterpret_cast(common_context->gid.data), - reinterpret_cast(&msg.gid.data), - RMW_GID_STORAGE_SIZE) == 0) - { - // ignore local messages - continue; - } - common_context->graph_cache.update_participant_entities(msg); - } - } - if (RMW_RET_OK != rmw_destroy_wait_set(wait_set)) { - TERMINATE("failed to destroy wait set"); - } - } -} diff --git a/rmw_fastrtps_cpp/src/rmw_init.cpp b/rmw_fastrtps_cpp/src/rmw_init.cpp index d04956b04..761422462 100644 --- a/rmw_fastrtps_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_cpp/src/rmw_init.cpp @@ -39,8 +39,6 @@ #include "rmw_fastrtps_cpp/publisher.hpp" #include "rmw_fastrtps_cpp/subscription.hpp" -#include "listener_thread.hpp" - extern "C" { rmw_ret_t diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index ffdc03fab..feba67d61 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -60,7 +60,6 @@ add_library(rmw_fastrtps_dynamic_cpp src/gid__type_support.cpp src/identifier.cpp src/init_rmw_context_impl.cpp - src/listener_thread.cpp src/node_entities_info__type_support.cpp src/participant_entities_info__type_support.cpp src/publisher.cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index 72bf4998b..a038d4bc7 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -36,7 +36,7 @@ #include "rosidl_typesupport_cpp/message_type_support.hpp" -#include "listener_thread.hpp" +#include "rmw_fastrtps_shared_cpp/listener_thread.hpp" using rmw_dds_common::msg::ParticipantEntitiesInfo; @@ -143,7 +143,7 @@ init_context_impl(rmw_context_t * context) context->impl->common = common_context.get(); context->impl->participant_info = participant_info.get(); - rmw_ret_t ret = rmw_fastrtps_dynamic_cpp::run_listener_thread(context); + rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); if (RMW_RET_OK != ret) { return ret; } @@ -188,7 +188,7 @@ rmw_fastrtps_dynamic_cpp::decrement_context_impl_ref_count(rmw_context_t * conte rmw_ret_t ret = RMW_RET_OK; rmw_error_string_t error_string; - ret = rmw_fastrtps_dynamic_cpp::join_listener_thread(context); + ret = rmw_fastrtps_shared_cpp::join_listener_thread(context); if (RMW_RET_OK != ret) { return ret; } diff --git a/rmw_fastrtps_dynamic_cpp/src/listener_thread.hpp b/rmw_fastrtps_dynamic_cpp/src/listener_thread.hpp deleted file mode 100644 index f66644788..000000000 --- a/rmw_fastrtps_dynamic_cpp/src/listener_thread.hpp +++ /dev/null @@ -1,30 +0,0 @@ -// 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 LISTENER_THREAD_HPP_ -#define LISTENER_THREAD_HPP_ - -#include "rmw/init.h" - -namespace rmw_fastrtps_dynamic_cpp -{ - -rmw_ret_t -run_listener_thread(rmw_context_t * context); - -rmw_ret_t -join_listener_thread(rmw_context_t * context); - -} // namespace rmw_fastrtps_dynamic_cpp -#endif // LISTENER_THREAD_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp index df59d96e2..8568b9021 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp @@ -39,8 +39,6 @@ #include "rmw_fastrtps_dynamic_cpp/publisher.hpp" #include "rmw_fastrtps_dynamic_cpp/subscription.hpp" -#include "listener_thread.hpp" - extern "C" { rmw_ret_t diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index 5e430e367..23a101050 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -52,6 +52,7 @@ add_library(rmw_fastrtps_shared_cpp src/custom_subscriber_info.cpp src/create_rmw_gid.cpp src/demangle.cpp + src/listener_thread.cpp src/namespace_prefix.cpp src/participant.cpp src/publisher.cpp diff --git a/rmw_fastrtps_cpp/src/listener_thread.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/listener_thread.hpp similarity index 70% rename from rmw_fastrtps_cpp/src/listener_thread.hpp rename to rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/listener_thread.hpp index f6b2cab9e..7b9eb6eeb 100644 --- a/rmw_fastrtps_cpp/src/listener_thread.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/listener_thread.hpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. +// 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LISTENER_THREAD_HPP_ -#define LISTENER_THREAD_HPP_ +#ifndef RMW_FASTRTPS_SHARED_CPP__LISTENER_THREAD_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__LISTENER_THREAD_HPP_ #include "rmw/init.h" -namespace rmw_fastrtps_cpp +namespace rmw_fastrtps_shared_cpp { rmw_ret_t @@ -26,5 +26,5 @@ run_listener_thread(rmw_context_t * context); rmw_ret_t join_listener_thread(rmw_context_t * context); -} // namespace rmw_fastrtps_cpp -#endif // LISTENER_THREAD_HPP_ +} // namespace rmw_fastrtps_shared_cpp +#endif // RMW_FASTRTPS_SHARED_CPP__LISTENER_THREAD_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp similarity index 78% rename from rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp rename to rmw_fastrtps_shared_cpp/src/listener_thread.cpp index 1bbfac93c..b3c38b533 100644 --- a/rmw_fastrtps_dynamic_cpp/src/listener_thread.cpp +++ b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. +// 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. @@ -31,10 +31,10 @@ #include "rmw_dds_common/gid_utils.hpp" #include "rmw_dds_common/msg/participant_entities_info.hpp" +#include "rmw_fastrtps_shared_cpp/listener_thread.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" -#include "listener_thread.hpp" - using rmw_dds_common::operator<<; static @@ -42,11 +42,12 @@ void node_listener(rmw_context_t * context); rmw_ret_t -rmw_fastrtps_dynamic_cpp::run_listener_thread(rmw_context_t * context) +rmw_fastrtps_shared_cpp::run_listener_thread(rmw_context_t * context) { auto common_context = static_cast(context->impl->common); common_context->thread_is_running.store(true); - common_context->listener_thread_gc = rmw_create_guard_condition(context); + common_context->listener_thread_gc = rmw_fastrtps_shared_cpp::__rmw_create_guard_condition( + context->implementation_identifier); if (common_context->listener_thread_gc) { try { common_context->listener_thread = std::thread(node_listener, context); @@ -61,7 +62,9 @@ rmw_fastrtps_dynamic_cpp::run_listener_thread(rmw_context_t * context) } common_context->thread_is_running.store(false); if (common_context->listener_thread_gc) { - if (RMW_RET_OK != rmw_destroy_guard_condition(common_context->listener_thread_gc)) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->listener_thread_gc)) + { RCUTILS_SAFE_FWRITE_TO_STDERR( RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); @@ -71,11 +74,12 @@ rmw_fastrtps_dynamic_cpp::run_listener_thread(rmw_context_t * context) } rmw_ret_t -rmw_fastrtps_dynamic_cpp::join_listener_thread(rmw_context_t * context) +rmw_fastrtps_shared_cpp::join_listener_thread(rmw_context_t * context) { auto common_context = static_cast(context->impl->common); common_context->thread_is_running.exchange(false); - rmw_ret_t rmw_ret = rmw_trigger_guard_condition(common_context->listener_thread_gc); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + context->implementation_identifier, common_context->listener_thread_gc); if (RMW_RET_OK != rmw_ret) { return rmw_ret; } @@ -88,7 +92,8 @@ rmw_fastrtps_dynamic_cpp::join_listener_thread(rmw_context_t * context) RMW_SET_ERROR_MSG("Failed to join std::thread"); return RMW_RET_ERROR; } - rmw_ret = rmw_destroy_guard_condition(common_context->listener_thread_gc); + rmw_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->listener_thread_gc); if (RMW_RET_OK != rmw_ret) { return rmw_ret; } @@ -122,11 +127,12 @@ node_listener(rmw_context_t * context) guard_conditions.guard_condition_count = 1; guard_conditions.guard_conditions = guard_conditions_buffer; // number of conditions of a subscription is 2 - rmw_wait_set_t * wait_set = rmw_create_wait_set(context, 2); + rmw_wait_set_t * wait_set = rmw_fastrtps_shared_cpp::__rmw_create_wait_set( + context->implementation_identifier, context, 2); if (nullptr == wait_set) { TERMINATE("failed to create wait set"); } - if (RMW_RET_OK != rmw_wait( + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_wait( &subscriptions, &guard_conditions, nullptr, @@ -140,13 +146,14 @@ node_listener(rmw_context_t * context) if (subscriptions_buffer[0]) { rmw_dds_common::msg::ParticipantEntitiesInfo msg; bool taken; - if (RMW_RET_OK != rmw_take( + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_take( + context->implementation_identifier, common_context->sub, static_cast(&msg), &taken, nullptr)) { - TERMINATE("rmw_take failed"); + TERMINATE("__rmw_take failed"); } if (taken) { if (std::memcmp( @@ -160,7 +167,9 @@ node_listener(rmw_context_t * context) common_context->graph_cache.update_participant_entities(msg); } } - if (RMW_RET_OK != rmw_destroy_wait_set(wait_set)) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( + context->implementation_identifier, wait_set)) + { TERMINATE("failed to destroy wait set"); } } From 162bd6cd73e7ef6821e9be1d39ff226de44eb6a4 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 5 Feb 2020 17:35:52 -0300 Subject: [PATCH 09/37] Get rid of some duplicated code in rmw_init.* Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/rmw_init.cpp | 76 ++---------- rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp | 76 ++---------- rmw_fastrtps_shared_cpp/CMakeLists.txt | 1 + .../rmw_fastrtps_shared_cpp/rmw_init.hpp | 38 ++++++ rmw_fastrtps_shared_cpp/src/rmw_init.cpp | 114 ++++++++++++++++++ 5 files changed, 167 insertions(+), 138 deletions(-) create mode 100644 rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_init.hpp create mode 100644 rmw_fastrtps_shared_cpp/src/rmw_init.cpp diff --git a/rmw_fastrtps_cpp/src/rmw_init.cpp b/rmw_fastrtps_cpp/src/rmw_init.cpp index 761422462..030ed958a 100644 --- a/rmw_fastrtps_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_cpp/src/rmw_init.cpp @@ -31,6 +31,7 @@ #include "rmw_fastrtps_shared_cpp/participant.hpp" #include "rmw_fastrtps_shared_cpp/publisher.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_init.hpp" #include "rmw_fastrtps_shared_cpp/subscription.hpp" #include "rosidl_typesupport_cpp/message_type_support.hpp" @@ -44,85 +45,22 @@ extern "C" rmw_ret_t rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t allocator) { - RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); - if (NULL != init_options->implementation_identifier) { - RMW_SET_ERROR_MSG("expected zero-initialized init_options"); - return RMW_RET_INVALID_ARGUMENT; - } - init_options->instance_id = 0; - init_options->implementation_identifier = eprosima_fastrtps_identifier; - init_options->allocator = allocator; - init_options->impl = nullptr; - init_options->name = rcutils_strdup("", allocator); - if (!init_options->name) { - RMW_SET_ERROR_MSG("failed to copy context name"); - return RMW_RET_BAD_ALLOC; - } - init_options->namespace_ = rcutils_strdup("", allocator); - if (!init_options->namespace_) { - allocator.deallocate(init_options->name, allocator.state); - RMW_SET_ERROR_MSG("failed to copy context namespace"); - return RMW_RET_BAD_ALLOC; - } - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::rmw_init_options_init( + eprosima_fastrtps_identifier, init_options, allocator); } rmw_ret_t rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) { - RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - src, - src->implementation_identifier, - eprosima_fastrtps_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (NULL != dst->implementation_identifier) { - RMW_SET_ERROR_MSG("expected zero-initialized dst"); - return RMW_RET_INVALID_ARGUMENT; - } - const rcutils_allocator_t * allocator = &src->allocator; - rmw_ret_t ret = RMW_RET_OK; - - *dst = *src; - dst->name = NULL; - dst->namespace_ = NULL; - dst->security_options = rmw_get_zero_initialized_security_options(); - - dst->name = rcutils_strdup(src->name, *allocator); - if (!dst->name) { - ret = RMW_RET_BAD_ALLOC; - goto fail; - } - dst->namespace_ = rcutils_strdup(src->namespace_, *allocator); - if (!dst->namespace_) { - ret = RMW_RET_BAD_ALLOC; - goto fail; - } - return rmw_security_options_copy(&src->security_options, allocator, &dst->security_options); -fail: - allocator->deallocate(dst->name, allocator->state); - allocator->deallocate(dst->namespace_, allocator->state); - return ret; + return rmw_fastrtps_shared_cpp::rmw_init_options_copy( + eprosima_fastrtps_identifier, src, dst); } rmw_ret_t rmw_init_options_fini(rmw_init_options_t * init_options) { - RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); - rcutils_allocator_t & allocator = init_options->allocator; - RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - init_options, - init_options->implementation_identifier, - eprosima_fastrtps_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - allocator.deallocate(init_options->name, allocator.state); - allocator.deallocate(init_options->namespace_, allocator.state); - rmw_security_options_fini(&init_options->security_options, &allocator); - *init_options = rmw_get_zero_initialized_init_options(); - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::rmw_init_options_fini( + eprosima_fastrtps_identifier, init_options); } using rmw_dds_common::msg::ParticipantEntitiesInfo; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp index 8568b9021..a7b549632 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp @@ -31,6 +31,7 @@ #include "rmw_fastrtps_shared_cpp/participant.hpp" #include "rmw_fastrtps_shared_cpp/publisher.hpp" #include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_init.hpp" #include "rmw_fastrtps_shared_cpp/subscription.hpp" #include "rosidl_typesupport_cpp/message_type_support.hpp" @@ -44,85 +45,22 @@ extern "C" rmw_ret_t rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t allocator) { - RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); - if (NULL != init_options->implementation_identifier) { - RMW_SET_ERROR_MSG("expected zero-initialized init_options"); - return RMW_RET_INVALID_ARGUMENT; - } - init_options->instance_id = 0; - init_options->implementation_identifier = eprosima_fastrtps_identifier; - init_options->allocator = allocator; - init_options->impl = nullptr; - init_options->name = rcutils_strdup("", allocator); - if (!init_options->name) { - RMW_SET_ERROR_MSG("failed to copy context name"); - return RMW_RET_BAD_ALLOC; - } - init_options->namespace_ = rcutils_strdup("", allocator); - if (!init_options->namespace_) { - allocator.deallocate(init_options->name, allocator.state); - RMW_SET_ERROR_MSG("failed to copy context namespace"); - return RMW_RET_BAD_ALLOC; - } - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::rmw_init_options_init( + eprosima_fastrtps_identifier, init_options, allocator); } rmw_ret_t rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) { - RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - src, - src->implementation_identifier, - eprosima_fastrtps_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (NULL != dst->implementation_identifier) { - RMW_SET_ERROR_MSG("expected zero-initialized dst"); - return RMW_RET_INVALID_ARGUMENT; - } - const rcutils_allocator_t * allocator = &src->allocator; - rmw_ret_t ret = RMW_RET_OK; - - *dst = *src; - dst->name = NULL; - dst->namespace_ = NULL; - dst->security_options = rmw_get_zero_initialized_security_options(); - - dst->name = rcutils_strdup(src->name, *allocator); - if (!dst->name) { - ret = RMW_RET_BAD_ALLOC; - goto fail; - } - dst->namespace_ = rcutils_strdup(src->namespace_, *allocator); - if (!dst->namespace_) { - ret = RMW_RET_BAD_ALLOC; - goto fail; - } - return rmw_security_options_copy(&src->security_options, allocator, &dst->security_options); -fail: - allocator->deallocate(dst->name, allocator->state); - allocator->deallocate(dst->namespace_, allocator->state); - return ret; + return rmw_fastrtps_shared_cpp::rmw_init_options_copy( + eprosima_fastrtps_identifier, src, dst); } rmw_ret_t rmw_init_options_fini(rmw_init_options_t * init_options) { - RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); - rcutils_allocator_t & allocator = init_options->allocator; - RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - init_options, - init_options->implementation_identifier, - eprosima_fastrtps_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - allocator.deallocate(init_options->name, allocator.state); - allocator.deallocate(init_options->namespace_, allocator.state); - rmw_security_options_fini(&init_options->security_options, &allocator); - *init_options = rmw_get_zero_initialized_init_options(); - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::rmw_init_options_fini( + eprosima_fastrtps_identifier, init_options); } using rmw_dds_common::msg::ParticipantEntitiesInfo; diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index 23a101050..23f97453f 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -64,6 +64,7 @@ add_library(rmw_fastrtps_shared_cpp src/rmw_get_gid_for_publisher.cpp src/rmw_get_topic_endpoint_info.cpp src/rmw_guard_condition.cpp + src/rmw_init.cpp src/rmw_logging.cpp src/rmw_node.cpp src/rmw_node_info_and_types.cpp diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_init.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_init.hpp new file mode 100644 index 000000000..8a17d2426 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_init.hpp @@ -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. + +#ifndef RMW_FASTRTPS_SHARED_CPP__RMW_INIT_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__RMW_INIT_HPP_ + +#include "rmw/init.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/init_options.h" + +namespace rmw_fastrtps_shared_cpp +{ + +rmw_ret_t +rmw_init_options_init( + const char * identifier, rmw_init_options_t * init_options, rcutils_allocator_t allocator); + +rmw_ret_t +rmw_init_options_copy( + const char * identifier, const rmw_init_options_t * src, rmw_init_options_t * dst); + +rmw_ret_t +rmw_init_options_fini(const char * identifier, rmw_init_options_t * init_options); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__RMW_INIT_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/rmw_init.cpp b/rmw_fastrtps_shared_cpp/src/rmw_init.cpp new file mode 100644 index 000000000..9041f6502 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_init.cpp @@ -0,0 +1,114 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// 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_fastrtps_shared_cpp/rmw_init.hpp" + +#include "rcutils/strdup.h" +#include "rcutils/types.h" + +#include "rmw/init.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/init_options.h" + +namespace rmw_fastrtps_shared_cpp +{ + +rmw_ret_t +rmw_init_options_init( + const char * identifier, rmw_init_options_t * init_options, rcutils_allocator_t allocator) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); + if (NULL != init_options->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized init_options"); + return RMW_RET_INVALID_ARGUMENT; + } + init_options->instance_id = 0; + init_options->implementation_identifier = identifier; + init_options->allocator = allocator; + init_options->impl = nullptr; + init_options->name = rcutils_strdup("", allocator); + if (!init_options->name) { + RMW_SET_ERROR_MSG("failed to copy context name"); + return RMW_RET_BAD_ALLOC; + } + init_options->namespace_ = rcutils_strdup("", allocator); + if (!init_options->namespace_) { + allocator.deallocate(init_options->name, allocator.state); + RMW_SET_ERROR_MSG("failed to copy context namespace"); + return RMW_RET_BAD_ALLOC; + } + return RMW_RET_OK; +} + +rmw_ret_t +rmw_init_options_copy( + const char * identifier, const rmw_init_options_t * src, rmw_init_options_t * dst) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + src, + src->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (NULL != dst->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized dst"); + return RMW_RET_INVALID_ARGUMENT; + } + const rcutils_allocator_t * allocator = &src->allocator; + rmw_ret_t ret = RMW_RET_OK; + + *dst = *src; + dst->name = NULL; + dst->namespace_ = NULL; + dst->security_options = rmw_get_zero_initialized_security_options(); + + dst->name = rcutils_strdup(src->name, *allocator); + if (!dst->name) { + ret = RMW_RET_BAD_ALLOC; + goto fail; + } + dst->namespace_ = rcutils_strdup(src->namespace_, *allocator); + if (!dst->namespace_) { + ret = RMW_RET_BAD_ALLOC; + goto fail; + } + return rmw_security_options_copy(&src->security_options, allocator, &dst->security_options); +fail: + allocator->deallocate(dst->name, allocator->state); + allocator->deallocate(dst->namespace_, allocator->state); + return ret; +} + +rmw_ret_t +rmw_init_options_fini(const char * identifier, rmw_init_options_t * init_options) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + rcutils_allocator_t & allocator = init_options->allocator; + RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + init_options, + init_options->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + allocator.deallocate(init_options->name, allocator.state); + allocator.deallocate(init_options->namespace_, allocator.state); + rmw_security_options_fini(&init_options->security_options, &allocator); + *init_options = rmw_get_zero_initialized_init_options(); + return RMW_RET_OK; +} + +} // namespace rmw_fastrtps_shared_cpp From fecf35bd64b7e01ab64f3f20da40ea4a30112626 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 5 Feb 2020 17:51:26 -0300 Subject: [PATCH 10/37] Get rid of some duplicated code in init_rmw_context_impl.* Signed-off-by: Ivan Santiago Paunovic --- .../init_rmw_context_impl.hpp | 7 -- .../src/init_rmw_context_impl.cpp | 61 ------------ rmw_fastrtps_cpp/src/rmw_node.cpp | 5 +- .../init_rmw_context_impl.hpp | 7 -- .../src/init_rmw_context_impl.cpp | 61 ------------ rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp | 5 +- rmw_fastrtps_shared_cpp/CMakeLists.txt | 1 + .../init_rmw_context_impl.hpp | 33 +++++++ .../src/init_rmw_context_impl.cpp | 92 +++++++++++++++++++ 9 files changed, 132 insertions(+), 140 deletions(-) create mode 100644 rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp create mode 100644 rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp index 7b9248999..b23183171 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp @@ -29,13 +29,6 @@ namespace rmw_fastrtps_cpp rmw_ret_t increment_context_impl_ref_count(rmw_context_t * context); -/// Increment `rmw_context_impl_t` reference count, destroying it if the count reaches zero. -/** - * Function that should be called when destroying a node. - */ -rmw_ret_t -decrement_context_impl_ref_count(rmw_context_t * context); - } // namespace rmw_fastrtps_cpp #endif // RMW_FASTRTPS_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index 34e49d329..2687d2000 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -173,64 +173,3 @@ rmw_fastrtps_cpp::increment_context_impl_ref_count(rmw_context_t * context) context->impl->count++; return RMW_RET_OK; } - -rmw_ret_t -rmw_fastrtps_cpp::decrement_context_impl_ref_count(rmw_context_t * context) -{ - std::lock_guard guard(context->impl->mutex); - - assert(context); - assert(context->impl); - assert(0u < context->impl->count); - - if (0u == --context->impl->count) { - rmw_ret_t err = RMW_RET_OK; - rmw_ret_t ret = RMW_RET_OK; - rmw_error_string_t error_string; - - ret = rmw_fastrtps_shared_cpp::join_listener_thread(context); - if (RMW_RET_OK != ret) { - return ret; - } - - auto common_context = static_cast(context->impl->common); - auto participant_info = static_cast(context->impl->participant_info); - - common_context->graph_cache.remove_participant(common_context->gid); - ret = rmw_fastrtps_shared_cpp::destroy_subscription( - eprosima_fastrtps_identifier, - participant_info, - common_context->sub); - // Try to clean the other objects if the above failed. - if (RMW_RET_OK != ret) { - error_string = rmw_get_error_string(); - rmw_reset_error(); - } - err = rmw_fastrtps_shared_cpp::destroy_publisher( - eprosima_fastrtps_identifier, - participant_info, - common_context->pub); - if (RMW_RET_OK != ret && RMW_RET_OK != err) { - // We can just return one error, log about the previous one. - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) - ": 'destroy_subscription' failed\n"); - ret = err; - error_string = rmw_get_error_string(); - rmw_reset_error(); - } - err = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); - if (RMW_RET_OK != ret && RMW_RET_OK != err) { - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) - ": 'destroy_publisher' failed\n"); - ret = err; - } else if (RMW_RET_OK != ret) { - RMW_SET_ERROR_MSG(error_string.str); - } - delete common_context; - context->impl->common = nullptr; - context->impl->participant_info = nullptr; - } - return RMW_RET_OK; -} diff --git a/rmw_fastrtps_cpp/src/rmw_node.cpp b/rmw_fastrtps_cpp/src/rmw_node.cpp index f6d4bfa6e..aa579fe2b 100644 --- a/rmw_fastrtps_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node.cpp @@ -27,6 +27,7 @@ #include "rmw/ret_types.h" #include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" @@ -62,7 +63,7 @@ rmw_create_node( context, eprosima_fastrtps_identifier, name, namespace_); if (nullptr == node) { - if (RMW_RET_OK != rmw_fastrtps_cpp::decrement_context_impl_ref_count(context)) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(context)) { RCUTILS_SAFE_FWRITE_TO_STDERR( "'decrement_context_impl_ref_count' failed while being executed due to '" RCUTILS_STRINGIFY(__function__) "' failing"); @@ -79,7 +80,7 @@ rmw_destroy_node(rmw_node_t * node) if (RMW_RET_OK != ret) { return ret; } - return rmw_fastrtps_cpp::decrement_context_impl_ref_count(node->context); + return rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(node->context); } rmw_ret_t diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp index eeeafcd65..07632f053 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp @@ -29,13 +29,6 @@ namespace rmw_fastrtps_dynamic_cpp rmw_ret_t increment_context_impl_ref_count(rmw_context_t * context); -/// Increment `rmw_context_impl_t` reference count, destroying it if the count reaches zero. -/** - * Function that should be called when destroying a node. - */ -rmw_ret_t -decrement_context_impl_ref_count(rmw_context_t * context); - } // namespace rmw_fastrtps_dynamic_cpp #endif // RMW_FASTRTPS_DYNAMIC_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index a038d4bc7..f76cb273a 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -173,64 +173,3 @@ rmw_fastrtps_dynamic_cpp::increment_context_impl_ref_count(rmw_context_t * conte context->impl->count++; return RMW_RET_OK; } - -rmw_ret_t -rmw_fastrtps_dynamic_cpp::decrement_context_impl_ref_count(rmw_context_t * context) -{ - std::lock_guard guard(context->impl->mutex); - - assert(context); - assert(context->impl); - assert(0u < context->impl->count); - - if (0u == --context->impl->count) { - rmw_ret_t err = RMW_RET_OK; - rmw_ret_t ret = RMW_RET_OK; - rmw_error_string_t error_string; - - ret = rmw_fastrtps_shared_cpp::join_listener_thread(context); - if (RMW_RET_OK != ret) { - return ret; - } - - auto common_context = static_cast(context->impl->common); - auto participant_info = static_cast(context->impl->participant_info); - - common_context->graph_cache.remove_participant(common_context->gid); - ret = rmw_fastrtps_shared_cpp::destroy_subscription( - eprosima_fastrtps_identifier, - participant_info, - common_context->sub); - // Try to clean the other objects if the above failed. - if (RMW_RET_OK != ret) { - error_string = rmw_get_error_string(); - rmw_reset_error(); - } - err = rmw_fastrtps_shared_cpp::destroy_publisher( - eprosima_fastrtps_identifier, - participant_info, - common_context->pub); - if (RMW_RET_OK != ret && RMW_RET_OK != err) { - // We can just return one error, log about the previous one. - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) - ": 'destroy_subscription' failed\n"); - ret = err; - error_string = rmw_get_error_string(); - rmw_reset_error(); - } - err = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); - if (RMW_RET_OK != ret && RMW_RET_OK != err) { - RMW_SAFE_FWRITE_TO_STDERR( - RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) - ": 'destroy_publisher' failed\n"); - ret = err; - } else if (RMW_RET_OK != ret) { - RMW_SET_ERROR_MSG(error_string.str); - } - delete common_context; - context->impl->common = nullptr; - context->impl->participant_info = nullptr; - } - return RMW_RET_OK; -} diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp index 6dea56047..78fd3e532 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp @@ -25,6 +25,7 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" @@ -56,7 +57,7 @@ rmw_create_node( context, eprosima_fastrtps_identifier, name, namespace_); if (nullptr == node) { - if (RMW_RET_OK != rmw_fastrtps_dynamic_cpp::decrement_context_impl_ref_count(context)) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(context)) { RCUTILS_SAFE_FWRITE_TO_STDERR( "'decrement_context_impl_ref_count' failed while being executed due to '" RCUTILS_STRINGIFY(__function__) "' failing"); @@ -73,7 +74,7 @@ rmw_destroy_node(rmw_node_t * node) if (RMW_RET_OK != ret) { return ret; } - return rmw_fastrtps_dynamic_cpp::decrement_context_impl_ref_count(node->context); + return rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(node->context); } rmw_ret_t diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index 23f97453f..c3f98a0f9 100644 --- a/rmw_fastrtps_shared_cpp/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/CMakeLists.txt @@ -52,6 +52,7 @@ add_library(rmw_fastrtps_shared_cpp src/custom_subscriber_info.cpp src/create_rmw_gid.cpp src/demangle.cpp + src/init_rmw_context_impl.cpp src/listener_thread.cpp src/namespace_prefix.cpp src/participant.cpp diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp new file mode 100644 index 000000000..c4dfe3881 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp @@ -0,0 +1,33 @@ +// 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 RMW_FASTRTPS_SHARED_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ + +#include "rmw/init.h" +#include "rmw/types.h" + +namespace rmw_fastrtps_shared_cpp +{ + +/// Increment `rmw_context_impl_t` reference count, destroying it if the count reaches zero. +/** + * Function that should be called when destroying a node. + */ +rmw_ret_t +decrement_context_impl_ref_count(rmw_context_t * context); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp new file mode 100644 index 000000000..04ebb3358 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp @@ -0,0 +1,92 @@ +// 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 "rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp" + +#include + +#include "rmw/error_handling.h" +#include "rmw/init.h" + +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/participant.hpp" +#include "rmw_fastrtps_shared_cpp/publisher.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +#include "rmw_fastrtps_shared_cpp/listener_thread.hpp" + +rmw_ret_t +rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(rmw_context_t * context) +{ + std::lock_guard guard(context->impl->mutex); + + assert(context); + assert(context->impl); + assert(0u < context->impl->count); + + if (0u == --context->impl->count) { + rmw_ret_t err = RMW_RET_OK; + rmw_ret_t ret = RMW_RET_OK; + rmw_error_string_t error_string; + + ret = rmw_fastrtps_shared_cpp::join_listener_thread(context); + if (RMW_RET_OK != ret) { + return ret; + } + + auto common_context = static_cast(context->impl->common); + auto participant_info = static_cast(context->impl->participant_info); + + common_context->graph_cache.remove_participant(common_context->gid); + ret = rmw_fastrtps_shared_cpp::destroy_subscription( + context->implementation_identifier, + participant_info, + common_context->sub); + // Try to clean the other objects if the above failed. + if (RMW_RET_OK != ret) { + error_string = rmw_get_error_string(); + rmw_reset_error(); + } + err = rmw_fastrtps_shared_cpp::destroy_publisher( + context->implementation_identifier, + participant_info, + common_context->pub); + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + // We can just return one error, log about the previous one. + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_subscription' failed\n"); + ret = err; + error_string = rmw_get_error_string(); + rmw_reset_error(); + } + err = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_publisher' failed\n"); + ret = err; + } else if (RMW_RET_OK != ret) { + RMW_SET_ERROR_MSG(error_string.str); + } + delete common_context; + context->impl->common = nullptr; + context->impl->participant_info = nullptr; + } + return RMW_RET_OK; +} From 87ff44983da2068b03606fda4cb2937f73795cc5 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 5 Feb 2020 18:24:14 -0300 Subject: [PATCH 11/37] Use new (std::nothrow) Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/rmw_init.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_init.cpp b/rmw_fastrtps_cpp/src/rmw_init.cpp index 030ed958a..5cd5927f3 100644 --- a/rmw_fastrtps_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_cpp/src/rmw_init.cpp @@ -86,7 +86,7 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) context->instance_id = options->instance_id; context->implementation_identifier = eprosima_fastrtps_identifier; - std::unique_ptr context_impl(new rmw_context_impl_t()); + std::unique_ptr context_impl(new (std::nothrow) rmw_context_impl_t()); if (!context_impl) { RMW_SET_ERROR_MSG("failed to allocate context impl"); return RMW_RET_BAD_ALLOC; diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp index a7b549632..9d320cfc1 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp @@ -86,8 +86,9 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context) context->instance_id = options->instance_id; context->implementation_identifier = eprosima_fastrtps_identifier; - std::unique_ptr context_impl(new rmw_context_impl_t()); + std::unique_ptr context_impl(new (std::nothrow) rmw_context_impl_t()); if (!context_impl) { + RMW_SET_ERROR_MSG("failed to allocate context impl"); return RMW_RET_BAD_ALLOC; } context->options = rmw_get_zero_initialized_init_options(); From bfbcef354e2e8a297fea117f867235a6033c7b5d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 5 Feb 2020 18:24:33 -0300 Subject: [PATCH 12/37] Correct bug in rmw_fastrtps_dynamic_cpp Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/rmw_node.cpp | 3 +-- rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp | 5 +++++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_node.cpp b/rmw_fastrtps_cpp/src/rmw_node.cpp index aa579fe2b..a36518f5d 100644 --- a/rmw_fastrtps_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2020 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -19,7 +19,6 @@ #include #include "rcutils/filesystem.h" -#include "rcutils/logging_macros.h" #include "rmw/allocators.h" #include "rmw/error_handling.h" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp index 78fd3e532..eca3ee4f8 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp @@ -1,3 +1,4 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -53,6 +54,10 @@ rmw_create_node( // TODO(wjwwood): replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when refactored return NULL); + if (RMW_RET_OK != rmw_fastrtps_dynamic_cpp::increment_context_impl_ref_count(context)) { + return nullptr; + } + rmw_node_t * node = rmw_fastrtps_shared_cpp::__rmw_create_node( context, eprosima_fastrtps_identifier, name, namespace_); From 39da7e32418d17eec7d99ea3069309fad2e429da Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 6 Feb 2020 13:38:18 -0300 Subject: [PATCH 13/37] Improve style Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/rmw_client.cpp | 4 ++-- rmw_fastrtps_cpp/src/rmw_publisher.cpp | 4 ++-- rmw_fastrtps_cpp/src/rmw_service.cpp | 4 ++-- rmw_fastrtps_cpp/src/rmw_subscription.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp | 4 ++-- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 4 ++-- rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp | 5 +++-- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 4 ++-- rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp | 4 ++-- 12 files changed, 25 insertions(+), 24 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index 6d2d53456..b9e846c51 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -221,8 +221,8 @@ rmw_create_client( node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.associate_reader( + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + msg = common_context->graph_cache.associate_reader( gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index 45557ec51..3af98bb62 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -92,8 +92,8 @@ rmw_create_publisher( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.associate_writer( + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + msg = common_context->graph_cache.associate_writer( info->publisher_gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index e1dbe4eac..51f695290 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -226,8 +226,8 @@ rmw_create_service( node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.associate_writer( + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + msg = common_context->graph_cache.associate_writer( gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp index 3a1d884ee..54aad0218 100644 --- a/rmw_fastrtps_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_cpp/src/rmw_subscription.cpp @@ -90,8 +90,8 @@ rmw_create_subscription( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.associate_reader( + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + msg = common_context->graph_cache.associate_reader( info->subscription_gid_, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 3a815e027..32344a8fc 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -251,8 +251,8 @@ rmw_create_client( node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.associate_reader( + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + msg = common_context->graph_cache.associate_reader( gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp index 6dbd0786c..c43d0593b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -96,8 +96,8 @@ rmw_create_publisher( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.associate_writer( + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + msg = common_context->graph_cache.associate_writer( info->publisher_gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 8f8b5648e..bc36ed01a 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -256,8 +256,8 @@ rmw_create_service( node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.associate_writer( + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + msg = common_context->graph_cache.associate_writer( gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp index 67d0583a0..f7a244f06 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -93,8 +93,8 @@ rmw_create_subscription( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.associate_reader( + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + msg = common_context->graph_cache.associate_reader( info->subscription_gid_, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index ec5237216..d7f89be8a 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -63,8 +63,8 @@ __rmw_destroy_client( node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( identifier, info->response_subscriber_->getGuid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.dissociate_reader( + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + msg = common_context->graph_cache.dissociate_reader( gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp index 0ee94394a..3203538c5 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -52,8 +52,9 @@ __rmw_destroy_publisher( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.dissociate_writer( + + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + msg = common_context->graph_cache.dissociate_writer( info->publisher_gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index f69a4f245..e73f53ba1 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -77,8 +77,8 @@ __rmw_destroy_service( node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( identifier, info->response_publisher_->getGuid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.dissociate_writer( + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + msg = common_context->graph_cache.dissociate_writer( gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index e8258472b..90e5c3b6b 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -54,8 +54,8 @@ __rmw_destroy_subscription( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg = - common_context->graph_cache.dissociate_reader( + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + msg = common_context->graph_cache.dissociate_reader( info->subscription_gid_, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, From 406e146065d510e7e77a805d43d2f3f0c28e9e4c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 6 Feb 2020 13:53:34 -0300 Subject: [PATCH 14/37] Make ros_discovery_info topic not accessible to ros users Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp | 6 +++--- rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index 2687d2000..12db45cf7 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -77,7 +77,7 @@ init_context_impl(rmw_context_t * context) rmw_qos_profile_t qos = rmw_qos_profile_default; - qos.avoid_ros_namespace_conventions = false; // Change this to true after testing + qos.avoid_ros_namespace_conventions = true; qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; qos.depth = 1; qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; @@ -88,7 +88,7 @@ init_context_impl(rmw_context_t * context) rmw_fastrtps_cpp::create_publisher( participant_info.get(), rosidl_typesupport_cpp::get_message_type_support_handle(), - "_participant_info", + "ros_discovery_info", &qos, &publisher_options, false, // our fastrtps typesupport doesn't support keyed topics @@ -115,7 +115,7 @@ init_context_impl(rmw_context_t * context) rmw_fastrtps_cpp::create_subscription( participant_info.get(), rosidl_typesupport_cpp::get_message_type_support_handle(), - "_participant_info", + "ros_discovery_info", &qos, &subscription_options, false, // our fastrtps typesupport doesn't support keyed topics diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index f76cb273a..ae45f1785 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -77,7 +77,7 @@ init_context_impl(rmw_context_t * context) rmw_qos_profile_t qos = rmw_qos_profile_default; - qos.avoid_ros_namespace_conventions = false; // Change this to true after testing + qos.avoid_ros_namespace_conventions = true; qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; qos.depth = 1; qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; @@ -88,7 +88,7 @@ init_context_impl(rmw_context_t * context) rmw_fastrtps_dynamic_cpp::create_publisher( participant_info.get(), rosidl_typesupport_cpp::get_message_type_support_handle(), - "_participant_info", + "ros_discovery_info", &qos, &publisher_options, false, // our fastrtps typesupport doesn't support keyed topics @@ -115,7 +115,7 @@ init_context_impl(rmw_context_t * context) rmw_fastrtps_dynamic_cpp::create_subscription( participant_info.get(), rosidl_typesupport_cpp::get_message_type_support_handle(), - "_participant_info", + "ros_discovery_info", &qos, &subscription_options, false, // our fastrtps typesupport doesn't support keyed topics From 5147650ca5ccaa3afe3ce462bbfea0a8ab9fabc2 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 6 Feb 2020 17:04:30 -0300 Subject: [PATCH 15/37] Add note explaining why we can't depend on rosidl_typesupport_cpp Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/gid__type_support.cpp | 4 ++++ rmw_fastrtps_cpp/src/node_entities_info__type_support.cpp | 4 ++++ .../src/participant_entities_info__type_support.cpp | 4 ++++ rmw_fastrtps_dynamic_cpp/src/gid__type_support.cpp | 4 ++++ .../src/node_entities_info__type_support.cpp | 4 ++++ .../src/participant_entities_info__type_support.cpp | 4 ++++ 6 files changed, 24 insertions(+) diff --git a/rmw_fastrtps_cpp/src/gid__type_support.cpp b/rmw_fastrtps_cpp/src/gid__type_support.cpp index 1e6d41ee2..1ace143fe 100644 --- a/rmw_fastrtps_cpp/src/gid__type_support.cpp +++ b/rmw_fastrtps_cpp/src/gid__type_support.cpp @@ -21,6 +21,10 @@ #include "rmw_dds_common/msg/gid__rosidl_typesupport_fastrtps_cpp.hpp" #include "rmw_fastrtps_cpp/visibility_control.h" +// This file is needed and can't be generated by `rosidl_typesupport_cpp`, +// because adding a dependency to that package generates a cyclic dependency +// (`rosidl_typesupport_cpp` exports a dependency on `rmw_implementation`). + namespace rosidl_typesupport_cpp { diff --git a/rmw_fastrtps_cpp/src/node_entities_info__type_support.cpp b/rmw_fastrtps_cpp/src/node_entities_info__type_support.cpp index b068dc273..b69a648b7 100644 --- a/rmw_fastrtps_cpp/src/node_entities_info__type_support.cpp +++ b/rmw_fastrtps_cpp/src/node_entities_info__type_support.cpp @@ -21,6 +21,10 @@ #include "rmw_dds_common/msg/node_entities_info__rosidl_typesupport_fastrtps_cpp.hpp" #include "rmw_fastrtps_cpp/visibility_control.h" +// This file is needed and can't be generated by `rosidl_typesupport_cpp`, +// because adding a dependency to that package generates a cyclic dependency +// (`rosidl_typesupport_cpp` exports a dependency on `rmw_implementation`). + namespace rosidl_typesupport_cpp { diff --git a/rmw_fastrtps_cpp/src/participant_entities_info__type_support.cpp b/rmw_fastrtps_cpp/src/participant_entities_info__type_support.cpp index ce27b9ad6..492581b73 100644 --- a/rmw_fastrtps_cpp/src/participant_entities_info__type_support.cpp +++ b/rmw_fastrtps_cpp/src/participant_entities_info__type_support.cpp @@ -21,6 +21,10 @@ #include "rmw_dds_common/msg/participant_entities_info__rosidl_typesupport_fastrtps_cpp.hpp" #include "rmw_fastrtps_cpp/visibility_control.h" +// This file is needed and can't be generated by `rosidl_typesupport_cpp`, +// because adding a dependency to that package generates a cyclic dependency +// (`rosidl_typesupport_cpp` exports a dependency on `rmw_implementation`). + namespace rosidl_typesupport_cpp { diff --git a/rmw_fastrtps_dynamic_cpp/src/gid__type_support.cpp b/rmw_fastrtps_dynamic_cpp/src/gid__type_support.cpp index 2e54e1d69..84c59cd28 100644 --- a/rmw_fastrtps_dynamic_cpp/src/gid__type_support.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/gid__type_support.cpp @@ -21,6 +21,10 @@ #include "rmw_dds_common/msg/gid__rosidl_typesupport_introspection_cpp.hpp" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" +// This file is needed and can't be generated by `rosidl_typesupport_cpp`, +// because adding a dependency to that package generates a cyclic dependency +// (`rosidl_typesupport_cpp` exports a dependency on `rmw_implementation`). + namespace rosidl_typesupport_cpp { diff --git a/rmw_fastrtps_dynamic_cpp/src/node_entities_info__type_support.cpp b/rmw_fastrtps_dynamic_cpp/src/node_entities_info__type_support.cpp index d03555f01..370935018 100644 --- a/rmw_fastrtps_dynamic_cpp/src/node_entities_info__type_support.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/node_entities_info__type_support.cpp @@ -21,6 +21,10 @@ #include "rmw_dds_common/msg/node_entities_info__rosidl_typesupport_introspection_cpp.hpp" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" +// This file is needed and can't be generated by `rosidl_typesupport_cpp`, +// because adding a dependency to that package generates a cyclic dependency +// (`rosidl_typesupport_cpp` exports a dependency on `rmw_implementation`). + namespace rosidl_typesupport_cpp { diff --git a/rmw_fastrtps_dynamic_cpp/src/participant_entities_info__type_support.cpp b/rmw_fastrtps_dynamic_cpp/src/participant_entities_info__type_support.cpp index c9d5cb3bf..151d79744 100644 --- a/rmw_fastrtps_dynamic_cpp/src/participant_entities_info__type_support.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/participant_entities_info__type_support.cpp @@ -21,6 +21,10 @@ #include "rmw_dds_common/msg/participant_entities_info__rosidl_typesupport_introspection_cpp.hpp" #include "rmw_fastrtps_dynamic_cpp/visibility_control.h" +// This file is needed and can't be generated by `rosidl_typesupport_cpp`, +// because adding a dependency to that package generates a cyclic dependency +// (`rosidl_typesupport_cpp` exports a dependency on `rmw_implementation`). + namespace rosidl_typesupport_cpp { From f9e9a021f3e8664d89d1b97aa4c10fd989ac8693 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 6 Feb 2020 17:05:55 -0300 Subject: [PATCH 16/37] Deleted old TODOs Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/publisher.cpp | 1 - rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 1 - 2 files changed, 2 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index c2fc1b642..4f5905a2c 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -88,7 +88,6 @@ rmw_fastrtps_cpp::create_publisher( // Load default XML profile. Domain::getDefaultPublisherAttributes(publisherParam); - // TODO(karsten1987) Verify consequences for std::unique_ptr? info = new (std::nothrow) CustomPublisherInfo(); if (!info) { RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 8bfb28923..8e068e019 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -100,7 +100,6 @@ rmw_fastrtps_dynamic_cpp::create_publisher( // Load default XML profile. Domain::getDefaultPublisherAttributes(publisherParam); - // TODO(karsten1987) Verify consequences for std::unique_ptr? info = new (std::nothrow) CustomPublisherInfo(); if (!info) { RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); From a366b7abac1640e7eeef1b18829315b10cb02a9c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 6 Feb 2020 17:11:14 -0300 Subject: [PATCH 17/37] Check return value of GraphCache::remove_participant Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp index 04ebb3358..69e0b8238 100644 --- a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp @@ -52,7 +52,11 @@ rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(rmw_context_t * contex auto common_context = static_cast(context->impl->common); auto participant_info = static_cast(context->impl->participant_info); - common_context->graph_cache.remove_participant(common_context->gid); + if (!common_context->graph_cache.remove_participant(common_context->gid)) { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " + "Couldn't remove Participant gid from graph_cache when destroying Participant"); + } ret = rmw_fastrtps_shared_cpp::destroy_subscription( context->implementation_identifier, participant_info, From 9c49cebc1e686c8b91900cc34316b90e9da0181a Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 6 Feb 2020 17:17:52 -0300 Subject: [PATCH 18/37] Return eprosima::fastrtps::string_255 instead of std::string in _mangle_topic_name Signed-off-by: Ivan Santiago Paunovic --- .../include/rmw_fastrtps_shared_cpp/names.hpp | 20 ++----------------- rmw_fastrtps_shared_cpp/src/rmw_count.cpp | 6 ++++-- .../src/rmw_get_topic_endpoint_info.cpp | 4 ++-- 3 files changed, 8 insertions(+), 22 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp index 6af152312..f4aff1c16 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp @@ -29,7 +29,7 @@ */ inline eprosima::fastrtps::string_255 -_mangle_topic_name_impl( +_mangle_topic_name( const char * prefix, const char * base, const char * suffix = nullptr) @@ -45,22 +45,6 @@ _mangle_topic_name_impl( return topicName.str(); } -/// Construct a topic name. -/** - * \param[in] prefix Required prefix for topic name. - * \param[in] base Required name of the topic. - * \param[in] suffix Optional suffix for topic name. - */ -inline -std::string -_mangle_topic_name( - const char * prefix, - const char * base, - const char * suffix = nullptr) -{ - return _mangle_topic_name_impl(prefix, base, suffix).to_string(); -} - /// Construct a topic name according to proper conventions. /** * \param[in] qos_profile The QoS profile for the topic. @@ -79,7 +63,7 @@ _create_topic_name( if (qos_profile->avoid_ros_namespace_conventions) { prefix = nullptr; } - return _mangle_topic_name_impl(prefix, base, suffix); + return _mangle_topic_name(prefix, base, suffix); } #endif // RMW_FASTRTPS_SHARED_CPP__NAMES_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/rmw_count.cpp b/rmw_fastrtps_shared_cpp/src/rmw_count.cpp index 902dc3776..a30d21989 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_count.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_count.cpp @@ -50,7 +50,8 @@ __rmw_count_publishers( return RMW_RET_INVALID_ARGUMENT; } auto common_context = static_cast(node->context->impl->common); - const std::string mangled_topic_name = _mangle_topic_name(ros_topic_prefix, topic_name); + const std::string mangled_topic_name = + _mangle_topic_name(ros_topic_prefix, topic_name).to_string(); return common_context->graph_cache.get_writer_count(mangled_topic_name, count); } @@ -70,7 +71,8 @@ __rmw_count_subscribers( return RMW_RET_INVALID_ARGUMENT; } auto common_context = static_cast(node->context->impl->common); - const std::string mangled_topic_name = _mangle_topic_name(ros_topic_prefix, topic_name); + const std::string mangled_topic_name = + _mangle_topic_name(ros_topic_prefix, topic_name).to_string(); return common_context->graph_cache.get_reader_count(mangled_topic_name, count); } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp index afff7af16..f9faf69e0 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -101,7 +101,7 @@ __rmw_get_publishers_info_by_topic( std::string mangled_topic_name = topic_name; DemangleFunction demangle_type = _identity_demangle; if (!no_mangle) { - mangled_topic_name = _mangle_topic_name(ros_topic_prefix, topic_name); + mangled_topic_name = _mangle_topic_name(ros_topic_prefix, topic_name).to_string(); demangle_type = _demangle_if_ros_type; } @@ -135,7 +135,7 @@ __rmw_get_subscriptions_info_by_topic( std::string mangled_topic_name = topic_name; DemangleFunction demangle_type = _identity_demangle; if (!no_mangle) { - mangled_topic_name = _mangle_topic_name(ros_topic_prefix, topic_name); + mangled_topic_name = _mangle_topic_name(ros_topic_prefix, topic_name).to_string(); demangle_type = _demangle_if_ros_type; } From d4543854299bb5ef71d071879b121b08fa3f237e Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 10 Feb 2020 17:49:50 -0300 Subject: [PATCH 19/37] Communicate context name and namespace to remote participants Signed-off-by: Ivan Santiago Paunovic --- .../src/init_rmw_context_impl.cpp | 7 +++- .../src/init_rmw_context_impl.cpp | 7 +++- .../custom_participant_info.hpp | 42 ++++++++++++++----- .../rmw_fastrtps_shared_cpp/participant.hpp | 2 + rmw_fastrtps_shared_cpp/src/participant.cpp | 15 ++++++- 5 files changed, 59 insertions(+), 14 deletions(-) diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index 12db45cf7..6b1640c4c 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -63,6 +63,8 @@ init_context_impl(rmw_context_t * context) context->options.domain_id, &context->options.security_options, (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, + context->options.name, + context->options.namespace_, common_context.get()), [&](CustomParticipantInfo * participant_info) { if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { @@ -147,7 +149,10 @@ init_context_impl(rmw_context_t * context) if (RMW_RET_OK != ret) { return ret; } - common_context->graph_cache.add_participant(common_context->gid); + common_context->graph_cache.add_participant( + common_context->gid, + context->options.name, + context->options.namespace_); publisher.release(); subscription.release(); diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index ae45f1785..9e6c0db32 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -63,6 +63,8 @@ init_context_impl(rmw_context_t * context) context->options.domain_id, &context->options.security_options, (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, + context->options.name, + context->options.namespace_, common_context.get()), [&](CustomParticipantInfo * participant_info) { if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { @@ -147,7 +149,10 @@ init_context_impl(rmw_context_t * context) if (RMW_RET_OK != ret) { return ret; } - common_context->graph_cache.add_participant(common_context->gid); + common_context->graph_cache.add_participant( + common_context->gid, + context->options.name, + context->options.namespace_); publisher.release(); subscription.release(); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index a63617dd8..de61b72a2 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -28,6 +28,7 @@ #include "rcpputils/thread_safety_annotations.hpp" #include "rcutils/logging_macros.h" +#include "rmw/impl/cpp/key_value.hpp" #include "rmw/qos_profiles.h" #include "rmw/rmw.h" @@ -69,17 +70,38 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener eprosima::fastrtps::Participant *, eprosima::fastrtps::rtps::ParticipantDiscoveryInfo && info) override { - // We aren't monitoring discovered participants, just dropped and removed. - // Participants are added to the Graph when they send a ParticipantEntitiesInfo message. - if ( - info.status != eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT && - info.status != eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT) - { - return; + switch (info.status) { + case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DISCOVERED_PARTICIPANT: + { + auto map = rmw::impl::cpp::parse_key_value(info.info.m_userData); + auto name_found = map.find("name"); + auto ns_found = map.find("namespace"); + + if (name_found == map.end() || ns_found == map.end()) { + return; + } + auto context_name = + std::string(name_found->second.begin(), name_found->second.end()); + auto context_namespace = + std::string(ns_found->second.begin(), ns_found->second.end()); + + context->graph_cache.add_participant( + rmw_fastrtps_shared_cpp::create_rmw_gid( + graph_guard_condition_->implementation_identifier, info.info.m_guid), + context_name, + context_namespace); + break; + } + case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT: + // fall through + case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT: + context->graph_cache.remove_participant( + rmw_fastrtps_shared_cpp::create_rmw_gid( + graph_guard_condition_->implementation_identifier, info.info.m_guid)); + break; + default: + return; } - context->graph_cache.remove_participant( - rmw_fastrtps_shared_cpp::create_rmw_gid( - graph_guard_condition_->implementation_identifier, info.info.m_guid)); } void onSubscriberDiscovery( diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp index b5608f632..5dfaebc10 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp @@ -30,6 +30,8 @@ create_participant( size_t domain_id, const rmw_security_options_t * security_options, bool localhost_only, + const char * context_name, + const char * context_namespace, rmw_dds_common::Context * common_context); rmw_ret_t diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index c3618fc7c..908ed716e 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -118,6 +118,8 @@ rmw_fastrtps_shared_cpp::create_participant( size_t domain_id, const rmw_security_options_t * security_options, bool localhost_only, + const char * context_name, + const char * context_namespace, rmw_dds_common::Context * common_context) { if (!security_options) { @@ -143,8 +145,17 @@ rmw_fastrtps_shared_cpp::create_participant( } participantAttrs.rtps.builtin.domainId = static_cast(domain_id); - // // since the participant name is not part of the DDS spec - // participantAttrs.rtps.setName(name); + + size_t length = strlen(context_name) + strlen("name=;") + + strlen(context_namespace) + strlen("namespace=;") + 1; + participantAttrs.rtps.userData.resize(length); + int written = snprintf( + reinterpret_cast(participantAttrs.rtps.userData.data()), + length, "name=%s;namespace=%s;", context_name, context_namespace); + if (written < 0 || written > static_cast(length) - 1) { + RMW_SET_ERROR_MSG("failed to populate user_data buffer"); + return nullptr; + } bool leave_middleware_default_qos = false; const char * env_value; From 6739de92e4cad9aaf42954a688480ff49f6e529e Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 11 Feb 2020 15:50:26 -0300 Subject: [PATCH 20/37] Trigger graph condition in any graph change Signed-off-by: Ivan Santiago Paunovic --- .../src/init_rmw_context_impl.cpp | 24 +++++++ .../src/init_rmw_context_impl.cpp | 25 ++++++++ .../custom_participant_info.hpp | 63 +++++++++---------- .../src/init_rmw_context_impl.cpp | 13 +++- rmw_fastrtps_shared_cpp/src/participant.cpp | 24 +------ rmw_fastrtps_shared_cpp/src/rmw_node.cpp | 8 +-- 6 files changed, 94 insertions(+), 63 deletions(-) diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index 6b1640c4c..c386f925b 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -137,10 +137,33 @@ init_context_impl(rmw_context_t * context) return RMW_RET_BAD_ALLOC; } + std::unique_ptr> + graph_guard_condition( + rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), + [&](rmw_guard_condition_t * p) { + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); + if (ret != RMW_RET_OK) { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy guard condition after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!graph_guard_condition) { + return RMW_RET_BAD_ALLOC; + } + + common_context->graph_cache.set_on_change_callback( + [guard_condition = graph_guard_condition.get()]() { + rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + eprosima_fastrtps_identifier, + guard_condition); + }); + common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, participant_info->participant->getGuid()); common_context->pub = publisher.get(); common_context->sub = subscription.get(); + common_context->graph_guard_condition = graph_guard_condition.get(); context->impl->common = common_context.get(); context->impl->participant_info = participant_info.get(); @@ -154,6 +177,7 @@ init_context_impl(rmw_context_t * context) context->options.name, context->options.namespace_); + graph_guard_condition.release(); publisher.release(); subscription.release(); common_context.release(); diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index 9e6c0db32..10ac60b86 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -137,10 +137,34 @@ init_context_impl(rmw_context_t * context) return RMW_RET_BAD_ALLOC; } + std::unique_ptr> + graph_guard_condition( + rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), + [&](rmw_guard_condition_t * p) { + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); + if (ret != RMW_RET_OK) { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy guard condition after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!graph_guard_condition) { + return RMW_RET_BAD_ALLOC; + } + + common_context->graph_cache.set_on_change_callback( + [guard_condition = graph_guard_condition.get()]() { + rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + eprosima_fastrtps_identifier, + guard_condition); + }); + + common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, participant_info->participant->getGuid()); common_context->pub = publisher.get(); common_context->sub = subscription.get(); + common_context->graph_guard_condition = graph_guard_condition.get(); context->impl->common = common_context.get(); context->impl->participant_info = participant_info.get(); @@ -154,6 +178,7 @@ init_context_impl(rmw_context_t * context) context->options.name, context->options.namespace_); + graph_guard_condition.release(); publisher.release(); subscription.release(); common_context.release(); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index de61b72a2..93bcfd650 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -46,7 +46,6 @@ typedef struct CustomParticipantInfo { eprosima::fastrtps::Participant * participant; ::ParticipantListener * listener; - rmw_guard_condition_t * graph_guard_condition; // Flag to establish if the QoS of the participant, // its publishers and its subscribers are going @@ -60,10 +59,10 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener { public: explicit ParticipantListener( - rmw_guard_condition_t * graph_guard_condition, + const char * identifier, rmw_dds_common::Context * context) : context(context), - graph_guard_condition_(graph_guard_condition) + identifier_(identifier) {} void onParticipantDiscovery( @@ -72,32 +71,32 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener { switch (info.status) { case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DISCOVERED_PARTICIPANT: - { - auto map = rmw::impl::cpp::parse_key_value(info.info.m_userData); - auto name_found = map.find("name"); - auto ns_found = map.find("namespace"); - - if (name_found == map.end() || ns_found == map.end()) { - return; - } - auto context_name = - std::string(name_found->second.begin(), name_found->second.end()); - auto context_namespace = - std::string(ns_found->second.begin(), ns_found->second.end()); - - context->graph_cache.add_participant( - rmw_fastrtps_shared_cpp::create_rmw_gid( - graph_guard_condition_->implementation_identifier, info.info.m_guid), + { + auto map = rmw::impl::cpp::parse_key_value(info.info.m_userData); + auto name_found = map.find("name"); + auto ns_found = map.find("namespace"); + + if (name_found == map.end() || ns_found == map.end()) { + return; + } + auto context_name = + std::string(name_found->second.begin(), name_found->second.end()); + auto context_namespace = + std::string(ns_found->second.begin(), ns_found->second.end()); + + context->graph_cache.add_participant( + rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier_, info.info.m_guid), context_name, context_namespace); - break; - } + break; + } case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT: - // fall through + // fall through case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT: context->graph_cache.remove_participant( rmw_fastrtps_shared_cpp::create_rmw_gid( - graph_guard_condition_->implementation_identifier, info.info.m_guid)); + identifier_, info.info.m_guid)); break; default: return; @@ -131,40 +130,34 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener void process_discovery_info(T & proxyData, bool is_alive, bool is_reader) { - bool trigger; { if (is_alive) { rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown; dds_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); - trigger = context->graph_cache.add_entity( + context->graph_cache.add_entity( rmw_fastrtps_shared_cpp::create_rmw_gid( - graph_guard_condition_->implementation_identifier, + identifier_, proxyData.guid()), proxyData.topicName().to_string(), proxyData.typeName().to_string(), rmw_fastrtps_shared_cpp::create_rmw_gid( - graph_guard_condition_->implementation_identifier, + identifier_, iHandle2GUID(proxyData.RTPSParticipantKey())), qos_profile, is_reader); } else { - trigger = context->graph_cache.remove_entity( + context->graph_cache.remove_entity( rmw_fastrtps_shared_cpp::create_rmw_gid( - graph_guard_condition_->implementation_identifier, + identifier_, proxyData.guid()), is_reader); } } - if (trigger) { - rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( - graph_guard_condition_->implementation_identifier, - graph_guard_condition_); - } } rmw_dds_common::Context * context; - rmw_guard_condition_t * graph_guard_condition_; + const char * const identifier_; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp index 69e0b8238..22c19a315 100644 --- a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp @@ -55,8 +55,9 @@ rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(rmw_context_t * contex if (!common_context->graph_cache.remove_participant(common_context->gid)) { RMW_SAFE_FWRITE_TO_STDERR( RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " - "Couldn't remove Participant gid from graph_cache when destroying Participant"); + "couldn't remove Participant gid from graph_cache when destroying Participant"); } + ret = rmw_fastrtps_shared_cpp::destroy_subscription( context->implementation_identifier, participant_info, @@ -89,6 +90,16 @@ rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(rmw_context_t * contex RMW_SET_ERROR_MSG(error_string.str); } delete common_context; + + common_context->graph_cache.clear_on_change_callback(); + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->graph_guard_condition)) + { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " + "couldn't destroy graph_guard_condtion"); + } + context->impl->common = nullptr; context->impl->participant_info = nullptr; } diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 908ed716e..3ec8daeb4 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -60,18 +60,11 @@ __create_participant( // Declare everything before beginning to create things. ::ParticipantListener * listener = nullptr; Participant * participant = nullptr; - rmw_guard_condition_t * graph_guard_condition = nullptr; CustomParticipantInfo * participant_info = nullptr; - graph_guard_condition = rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(identifier); - if (!graph_guard_condition) { - // error already set - goto fail; - } - try { listener = new ::ParticipantListener( - graph_guard_condition, common_context); + identifier, common_context); } catch (std::bad_alloc &) { RMW_SET_ERROR_MSG("failed to allocate participant listener"); goto fail; @@ -93,18 +86,9 @@ __create_participant( participant_info->participant = participant; participant_info->listener = listener; - participant_info->graph_guard_condition = graph_guard_condition; return participant_info; fail: - if (graph_guard_condition) { - rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(graph_guard_condition); - if (ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_shared_cpp", - "failed to destroy guard condition during error handling"); - } - } rmw_free(listener); if (participant) { Domain::removeParticipant(participant); @@ -238,12 +222,6 @@ rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant return RMW_RET_ERROR; } Domain::removeParticipant(participant_info->participant); - if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( - participant_info->graph_guard_condition)) - { - RMW_SET_ERROR_MSG("failed to destroy graph guard condition"); - result_ret = RMW_RET_ERROR; - } delete participant_info->listener; participant_info->listener = nullptr; delete participant_info; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp index 080e61cd0..bd1794c95 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp @@ -167,11 +167,11 @@ __rmw_node_assert_liveliness( const rmw_guard_condition_t * __rmw_node_get_graph_guard_condition(const rmw_node_t * node) { - auto impl = static_cast(node->context->impl->participant_info); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); + auto common_context = static_cast(node->context->impl->common); + if (!common_context) { + RMW_SET_ERROR_MSG("common_context is nullptr"); return nullptr; } - return impl->graph_guard_condition; + return common_context->graph_guard_condition; } } // namespace rmw_fastrtps_shared_cpp From 2eab379b8d14f940157f21ec76854b571bcbdf2c Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 28 Feb 2020 11:30:49 -0300 Subject: [PATCH 21/37] Address peer review comments Signed-off-by: Ivan Santiago Paunovic --- .../include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp | 5 ++--- rmw_fastrtps_cpp/src/subscription.cpp | 7 ++++--- .../rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp | 5 ++--- .../include/rmw_fastrtps_shared_cpp/participant.hpp | 1 + .../src/rmw_get_topic_endpoint_info.cpp | 6 ++---- 5 files changed, 11 insertions(+), 13 deletions(-) diff --git a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp index b23183171..d9e2724e3 100644 --- a/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp @@ -21,10 +21,9 @@ namespace rmw_fastrtps_cpp { -/// Increment `rmw_context_impl_t` reference count, initing it necessary. +/// Increment `rmw_context_impl_t` reference count, initializing it if necessary. /** - * Function that should be called when creating a node, - * before using `context->impl`. + * Should be called when creating a node, and before using `context->impl`. */ rmw_ret_t increment_context_impl_ref_count(rmw_context_t * context); diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp index 4e0941052..6e1122404 100644 --- a/rmw_fastrtps_cpp/src/subscription.cpp +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -15,6 +15,9 @@ #include #include +#include "rcutils/allocator.h" +#include "rcutils/strdup.h" + #include "rmw/allocators.h" #include "rmw/error_handling.h" #include "rmw/rmw.h" @@ -150,13 +153,11 @@ create_subscription( } rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; rmw_subscription->data = info; - rmw_subscription->topic_name = - reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + rmw_subscription->topic_name = rcutils_strdup(topic_name, rcutils_get_default_allocator()); if (!rmw_subscription->topic_name) { RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); goto fail; } - memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); rmw_subscription->options = *subscription_options; return rmw_subscription; diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp index 07632f053..11cc71da4 100644 --- a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp @@ -21,10 +21,9 @@ namespace rmw_fastrtps_dynamic_cpp { -/// Increment `rmw_context_impl_t` reference count, initing it necessary. +/// Increment `rmw_context_impl_t` reference count, initializing it if necessary. /** - * Function that should be called when creating a node, - * before using `context->impl`. + * Should be called when creating a node, and before using `context->impl`. */ rmw_ret_t increment_context_impl_ref_count(rmw_context_t * context); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp index 5dfaebc10..9c18fd966 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp @@ -36,6 +36,7 @@ create_participant( rmw_ret_t destroy_participant(CustomParticipantInfo * info); + } // namespace rmw_fastrtps_shared_cpp #endif // RMW_FASTRTPS_SHARED_CPP__PARTICIPANT_HPP_ diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp index f9faf69e0..c6cd2bbda 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -97,7 +97,6 @@ __rmw_get_publishers_info_by_topic( return ret; } auto common_context = static_cast(node->context->impl->common); - rmw_dds_common::GraphCache & graph_cache = common_context->graph_cache; std::string mangled_topic_name = topic_name; DemangleFunction demangle_type = _identity_demangle; if (!no_mangle) { @@ -105,7 +104,7 @@ __rmw_get_publishers_info_by_topic( demangle_type = _demangle_if_ros_type; } - return graph_cache.get_writers_info_by_topic( + return common_context->graph_cache.get_writers_info_by_topic( mangled_topic_name, demangle_type, allocator, @@ -131,7 +130,6 @@ __rmw_get_subscriptions_info_by_topic( return ret; } auto common_context = static_cast(node->context->impl->common); - rmw_dds_common::GraphCache & graph_cache = common_context->graph_cache; std::string mangled_topic_name = topic_name; DemangleFunction demangle_type = _identity_demangle; if (!no_mangle) { @@ -139,7 +137,7 @@ __rmw_get_subscriptions_info_by_topic( demangle_type = _demangle_if_ros_type; } - return graph_cache.get_readers_info_by_topic( + return common_context->graph_cache.get_readers_info_by_topic( mangled_topic_name, demangle_type, allocator, From 16a3d24bc431d27c13297bb3c633b1ece4718ccb Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 28 Feb 2020 11:34:13 -0300 Subject: [PATCH 22/37] Revert "Improve style" This reverts commit 4d1470f1cdf452b2b314bbc8f1541a347ba2fd1a. Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/rmw_client.cpp | 4 ++-- rmw_fastrtps_cpp/src/rmw_publisher.cpp | 4 ++-- rmw_fastrtps_cpp/src/rmw_service.cpp | 4 ++-- rmw_fastrtps_cpp/src/rmw_subscription.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp | 4 ++-- rmw_fastrtps_shared_cpp/src/rmw_client.cpp | 4 ++-- rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp | 5 ++--- rmw_fastrtps_shared_cpp/src/rmw_service.cpp | 4 ++-- rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp | 4 ++-- 12 files changed, 24 insertions(+), 25 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index b9e846c51..6d2d53456 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -221,8 +221,8 @@ rmw_create_client( node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - msg = common_context->graph_cache.associate_reader( + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index 3af98bb62..45557ec51 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -92,8 +92,8 @@ rmw_create_publisher( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - msg = common_context->graph_cache.associate_writer( + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_writer( info->publisher_gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 51f695290..e1dbe4eac 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -226,8 +226,8 @@ rmw_create_service( node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - msg = common_context->graph_cache.associate_writer( + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_writer( gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp index 54aad0218..3a1d884ee 100644 --- a/rmw_fastrtps_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_cpp/src/rmw_subscription.cpp @@ -90,8 +90,8 @@ rmw_create_subscription( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - msg = common_context->graph_cache.associate_reader( + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( info->subscription_gid_, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 32344a8fc..3a815e027 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -251,8 +251,8 @@ rmw_create_client( node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - msg = common_context->graph_cache.associate_reader( + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp index c43d0593b..6dbd0786c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -96,8 +96,8 @@ rmw_create_publisher( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - msg = common_context->graph_cache.associate_writer( + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_writer( info->publisher_gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index bc36ed01a..8f8b5648e 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -256,8 +256,8 @@ rmw_create_service( node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - msg = common_context->graph_cache.associate_writer( + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_writer( gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp index f7a244f06..67d0583a0 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -93,8 +93,8 @@ rmw_create_subscription( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - msg = common_context->graph_cache.associate_reader( + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( info->subscription_gid_, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( eprosima_fastrtps_identifier, diff --git a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index d7f89be8a..ec5237216 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -63,8 +63,8 @@ __rmw_destroy_client( node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( identifier, info->response_subscriber_->getGuid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - msg = common_context->graph_cache.dissociate_reader( + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp index 3203538c5..0ee94394a 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -52,9 +52,8 @@ __rmw_destroy_publisher( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - msg = common_context->graph_cache.dissociate_writer( + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_writer( info->publisher_gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, diff --git a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp index e73f53ba1..f69a4f245 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_service.cpp @@ -77,8 +77,8 @@ __rmw_destroy_service( node->namespace_); gid = rmw_fastrtps_shared_cpp::create_rmw_gid( identifier, info->response_publisher_->getGuid()); - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - msg = common_context->graph_cache.dissociate_writer( + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_writer( gid, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index 90e5c3b6b..e8258472b 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -54,8 +54,8 @@ __rmw_destroy_subscription( { // Update graph std::lock_guard guard(common_context->node_update_mutex); - rmw_dds_common::msg::ParticipantEntitiesInfo msg; - msg = common_context->graph_cache.dissociate_reader( + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( info->subscription_gid_, common_context->gid, node->name, node->namespace_); rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( identifier, From 64da434873a2023ffec36d7865af21d8a3de14dc Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 28 Feb 2020 12:47:26 -0300 Subject: [PATCH 23/37] Address peer review comments Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp | 9 +++++++++ rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp | 9 +++++++++ 2 files changed, 18 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp index 0ee94394a..99b45bac1 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_publisher.cpp @@ -47,6 +47,15 @@ __rmw_destroy_publisher( return RMW_RET_ERROR; } + if (!publisher) { + RMW_SET_ERROR_MSG("publisher handle is null"); + return RMW_RET_ERROR; + } + if (publisher->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("publisher handle not from this implementation"); + return RMW_RET_ERROR; + } + auto common_context = static_cast(node->context->impl->common); auto info = static_cast(publisher->data); { diff --git a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp index e8258472b..ae5932b23 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_subscription.cpp @@ -49,6 +49,15 @@ __rmw_destroy_subscription( return RMW_RET_ERROR; } + if (!subscription) { + RMW_SET_ERROR_MSG("subscription handle is null"); + return RMW_RET_ERROR; + } + if (subscription->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("subscription handle not from this implementation"); + return RMW_RET_ERROR; + } + auto common_context = static_cast(node->context->impl->common); auto info = static_cast(subscription->data); { From a0e29db970fbf23606c00a9c7835d5f53fee9555 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 28 Feb 2020 14:59:13 -0300 Subject: [PATCH 24/37] Update after rebasing Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 16 ++-------------- rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp | 4 ++-- 2 files changed, 4 insertions(+), 16 deletions(-) diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 8e068e019..20fa3bceb 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -95,7 +95,6 @@ rmw_fastrtps_dynamic_cpp::create_publisher( CustomPublisherInfo * info = nullptr; rmw_publisher_t * rmw_publisher = nullptr; eprosima::fastrtps::PublisherAttributes publisherParam; - const eprosima::fastrtps::rtps::GUID_t * guid = nullptr; // Load default XML profile. Domain::getDefaultPublisherAttributes(publisherParam); @@ -167,19 +166,8 @@ rmw_fastrtps_dynamic_cpp::create_publisher( goto fail; } - info->publisher_gid.implementation_identifier = eprosima_fastrtps_identifier; - static_assert( - sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, - "RMW_GID_STORAGE_SIZE insufficient to store the rmw_fastrtps_dynamic_cpp GID implementation." - ); - - memset(info->publisher_gid.data, 0, RMW_GID_STORAGE_SIZE); - guid = &info->publisher_->getGuid(); - if (!guid) { - RMW_SET_ERROR_MSG("no guid found for publisher"); - goto fail; - } - memcpy(info->publisher_gid.data, guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); + info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->publisher_->getGuid()); rmw_publisher = rmw_publisher_allocate(); if (!rmw_publisher) { diff --git a/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp b/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp index b7de21922..72ffb3087 100644 --- a/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp +++ b/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp @@ -30,7 +30,7 @@ rmw_fastrtps_shared_cpp::create_rmw_gid( "RMW_GID_STORAGE_SIZE insufficient to store the fastrtps GUID_t." ); rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( - info->publisher_->getGuid(), - info->publisher_gid.data); + guid, + rmw_gid.data); return rmw_gid; } From aad980ac904e63eea9c4384b20c42b1aec3dfff3 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 28 Feb 2020 18:17:45 -0300 Subject: [PATCH 25/37] Restore get_security_file_paths Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_shared_cpp/src/participant.cpp | 38 +++++++++++++++++++++ rmw_fastrtps_shared_cpp/src/rmw_node.cpp | 1 - 2 files changed, 38 insertions(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 3ec8daeb4..ffac8f2eb 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -34,6 +34,7 @@ #include "fastrtps/subscriber/SubscriberListener.h" #include "fastrtps/subscriber/SampleInfo.h" +#include "rcutils/filesystem.h" #include "rcutils/get_env.h" #include "rmw/allocators.h" @@ -49,6 +50,43 @@ using Participant = eprosima::fastrtps::Participant; using ParticipantAttributes = eprosima::fastrtps::ParticipantAttributes; using StatefulReader = eprosima::fastrtps::rtps::StatefulReader; +#if HAVE_SECURITY +static +bool +get_security_file_paths( + std::array & security_files_paths, const char * node_secure_root) +{ + // here assume only 6 files for security + const char * file_names[6] = { + "identity_ca.cert.pem", "cert.pem", "key.pem", + "permissions_ca.cert.pem", "governance.p7s", "permissions.p7s" + }; + size_t num_files = sizeof(file_names) / sizeof(char *); + + std::string file_prefix("file://"); + + for (size_t i = 0; i < num_files; i++) { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + char * file_path = rcutils_join_path(node_secure_root, file_names[i], allocator); + + if (!file_path) { + return false; + } + + if (rcutils_is_readable(file_path)) { + security_files_paths[i] = file_prefix + std::string(file_path); + } else { + allocator.deallocate(file_path, allocator.state); + return false; + } + + allocator.deallocate(file_path, allocator.state); + } + + return true; +} +#endif + static CustomParticipantInfo * __create_participant( diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp index bd1794c95..d59789289 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp @@ -19,7 +19,6 @@ #include #include -#include "rcutils/filesystem.h" #include "rcutils/logging_macros.h" #include "rmw/allocators.h" From 5499f073ac67311a4bd7c1d0c4763fbfb23e3cc7 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 12 Mar 2020 11:33:59 -0300 Subject: [PATCH 26/37] Latest update after discussion about supporting sros2 Signed-off-by: Ivan Santiago Paunovic --- .../src/init_rmw_context_impl.cpp | 4 +-- rmw_fastrtps_cpp/src/rmw_node.cpp | 2 -- rmw_fastrtps_cpp/src/rmw_node_names.cpp | 11 ++++++ .../src/init_rmw_context_impl.cpp | 4 +-- rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp | 2 -- .../src/rmw_node_names.cpp | 11 ++++++ .../custom_participant_info.hpp | 10 ++---- .../rmw_fastrtps_shared_cpp/participant.hpp | 1 - .../rmw_fastrtps_shared_cpp/rmw_common.hpp | 9 +++++ rmw_fastrtps_shared_cpp/src/participant.cpp | 12 +++---- rmw_fastrtps_shared_cpp/src/rmw_init.cpp | 26 ++++---------- .../src/rmw_node_names.cpp | 36 +++++++++++++++++++ 12 files changed, 83 insertions(+), 45 deletions(-) diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index c386f925b..daf6b3b09 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -64,7 +64,6 @@ init_context_impl(rmw_context_t * context) &context->options.security_options, (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, context->options.name, - context->options.namespace_, common_context.get()), [&](CustomParticipantInfo * participant_info) { if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { @@ -174,8 +173,7 @@ init_context_impl(rmw_context_t * context) } common_context->graph_cache.add_participant( common_context->gid, - context->options.name, - context->options.namespace_); + context->options.name); graph_guard_condition.release(); publisher.release(); diff --git a/rmw_fastrtps_cpp/src/rmw_node.cpp b/rmw_fastrtps_cpp/src/rmw_node.cpp index a36518f5d..bad5bebda 100644 --- a/rmw_fastrtps_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node.cpp @@ -40,11 +40,9 @@ rmw_create_node( const char * name, const char * namespace_, size_t domain_id, - const rmw_security_options_t * security_options, bool localhost_only) { (void)domain_id; - (void)security_options; (void)localhost_only; RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( diff --git a/rmw_fastrtps_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_cpp/src/rmw_node_names.cpp index bcdbe6648..6ea7a875d 100644 --- a/rmw_fastrtps_cpp/src/rmw_node_names.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node_names.cpp @@ -39,4 +39,15 @@ rmw_get_node_names( return rmw_fastrtps_shared_cpp::__rmw_get_node_names( eprosima_fastrtps_identifier, node, node_names, node_namespaces); } + +rmw_ret_t +rmw_get_node_names_with_context_names( + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * context_names) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_node_names_with_context_names( + eprosima_fastrtps_identifier, node, node_names, node_namespaces, context_names); +} } // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index 10ac60b86..b712fad12 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -64,7 +64,6 @@ init_context_impl(rmw_context_t * context) &context->options.security_options, (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, context->options.name, - context->options.namespace_, common_context.get()), [&](CustomParticipantInfo * participant_info) { if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { @@ -175,8 +174,7 @@ init_context_impl(rmw_context_t * context) } common_context->graph_cache.add_participant( common_context->gid, - context->options.name, - context->options.namespace_); + context->options.name); graph_guard_condition.release(); publisher.release(); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp index eca3ee4f8..2a0a629e2 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp @@ -40,11 +40,9 @@ rmw_create_node( const char * name, const char * namespace_, size_t domain_id, - const rmw_security_options_t * security_options, bool localhost_only) { (void)domain_id; - (void)security_options; (void)localhost_only; RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp index 55440d33a..b5f1099c5 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp @@ -39,4 +39,15 @@ rmw_get_node_names( return rmw_fastrtps_shared_cpp::__rmw_get_node_names( eprosima_fastrtps_identifier, node, node_names, node_namespaces); } + +rmw_ret_t +rmw_get_node_names_with_context_names( + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * context_names) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_node_names_with_context_names( + eprosima_fastrtps_identifier, node, node_names, node_namespaces, context_names); +} } // extern "C" diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 93bcfd650..e96b0f65f 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -73,22 +73,18 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DISCOVERED_PARTICIPANT: { auto map = rmw::impl::cpp::parse_key_value(info.info.m_userData); - auto name_found = map.find("name"); - auto ns_found = map.find("namespace"); + auto name_found = map.find("contextname"); - if (name_found == map.end() || ns_found == map.end()) { + if (name_found == map.end()) { return; } auto context_name = std::string(name_found->second.begin(), name_found->second.end()); - auto context_namespace = - std::string(ns_found->second.begin(), ns_found->second.end()); context->graph_cache.add_participant( rmw_fastrtps_shared_cpp::create_rmw_gid( identifier_, info.info.m_guid), - context_name, - context_namespace); + context_name); break; } case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT: diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp index 9c18fd966..18f8ba658 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp @@ -31,7 +31,6 @@ create_participant( const rmw_security_options_t * security_options, bool localhost_only, const char * context_name, - const char * context_namespace, rmw_dds_common::Context * common_context); rmw_ret_t diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp index 1ef7e2ac3..ecd541012 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -124,6 +124,15 @@ __rmw_init_event( void * data, rmw_event_type_t event_type); +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_node_names_with_context_names( + const char * identifier, + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * context_names); + RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t __rmw_publish( diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index ffac8f2eb..f6b822f7a 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -54,7 +54,7 @@ using StatefulReader = eprosima::fastrtps::rtps::StatefulReader; static bool get_security_file_paths( - std::array & security_files_paths, const char * node_secure_root) + std::array & security_files_paths, const char * secure_root) { // here assume only 6 files for security const char * file_names[6] = { @@ -67,7 +67,7 @@ get_security_file_paths( for (size_t i = 0; i < num_files; i++) { rcutils_allocator_t allocator = rcutils_get_default_allocator(); - char * file_path = rcutils_join_path(node_secure_root, file_names[i], allocator); + char * file_path = rcutils_join_path(secure_root, file_names[i], allocator); if (!file_path) { return false; @@ -141,7 +141,6 @@ rmw_fastrtps_shared_cpp::create_participant( const rmw_security_options_t * security_options, bool localhost_only, const char * context_name, - const char * context_namespace, rmw_dds_common::Context * common_context) { if (!security_options) { @@ -168,16 +167,16 @@ rmw_fastrtps_shared_cpp::create_participant( participantAttrs.rtps.builtin.domainId = static_cast(domain_id); - size_t length = strlen(context_name) + strlen("name=;") + - strlen(context_namespace) + strlen("namespace=;") + 1; + size_t length = strlen(context_name) + strlen("contextname=;") + 1; participantAttrs.rtps.userData.resize(length); int written = snprintf( reinterpret_cast(participantAttrs.rtps.userData.data()), - length, "name=%s;namespace=%s;", context_name, context_namespace); + length, "contextname=%s;", context_name); if (written < 0 || written > static_cast(length) - 1) { RMW_SET_ERROR_MSG("failed to populate user_data buffer"); return nullptr; } + participantAttrs.rtps.setName(context_name); bool leave_middleware_default_qos = false; const char * env_value; @@ -201,7 +200,6 @@ rmw_fastrtps_shared_cpp::create_participant( // if security_root_path provided, try to find the key and certificate files #if HAVE_SECURITY std::array security_files_paths; - if (get_security_file_paths(security_files_paths, security_options->security_root_path)) { eprosima::fastrtps::rtps::PropertyPolicy property_policy; using Property = eprosima::fastrtps::rtps::Property; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_init.cpp b/rmw_fastrtps_shared_cpp/src/rmw_init.cpp index 9041f6502..6b783d0cd 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_init.cpp @@ -39,17 +39,10 @@ rmw_init_options_init( init_options->implementation_identifier = identifier; init_options->allocator = allocator; init_options->impl = nullptr; - init_options->name = rcutils_strdup("", allocator); - if (!init_options->name) { - RMW_SET_ERROR_MSG("failed to copy context name"); - return RMW_RET_BAD_ALLOC; - } - init_options->namespace_ = rcutils_strdup("", allocator); - if (!init_options->namespace_) { - allocator.deallocate(init_options->name, allocator.state); - RMW_SET_ERROR_MSG("failed to copy context namespace"); - return RMW_RET_BAD_ALLOC; - } + init_options->name = NULL; + init_options->domain_id = RMW_DEFAULT_DOMAIN_ID; + init_options->security_options = rmw_get_default_security_options(); + init_options->localhost_only = RMW_LOCALHOST_ONLY_DEFAULT; return RMW_RET_OK; } @@ -71,25 +64,19 @@ rmw_init_options_copy( const rcutils_allocator_t * allocator = &src->allocator; rmw_ret_t ret = RMW_RET_OK; + allocator->deallocate(dst->name, allocator->state); *dst = *src; dst->name = NULL; - dst->namespace_ = NULL; dst->security_options = rmw_get_zero_initialized_security_options(); dst->name = rcutils_strdup(src->name, *allocator); - if (!dst->name) { - ret = RMW_RET_BAD_ALLOC; - goto fail; - } - dst->namespace_ = rcutils_strdup(src->namespace_, *allocator); - if (!dst->namespace_) { + if (src->name && !dst->name) { ret = RMW_RET_BAD_ALLOC; goto fail; } return rmw_security_options_copy(&src->security_options, allocator, &dst->security_options); fail: allocator->deallocate(dst->name, allocator->state); - allocator->deallocate(dst->namespace_, allocator->state); return ret; } @@ -105,7 +92,6 @@ rmw_init_options_fini(const char * identifier, rmw_init_options_t * init_options identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); allocator.deallocate(init_options->name, allocator.state); - allocator.deallocate(init_options->namespace_, allocator.state); rmw_security_options_fini(&init_options->security_options, &allocator); *init_options = rmw_get_zero_initialized_init_options(); return RMW_RET_OK; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp index 846ef7819..4e0d8e07c 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp @@ -64,6 +64,42 @@ __rmw_get_node_names( return common_context->graph_cache.get_node_names( node_names, node_namespaces, + nullptr, + &allocator); +} + +rmw_ret_t +__rmw_get_node_names_with_context_names( + const char * identifier, + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * context_names) +{ + if (!node) { + RMW_SET_ERROR_MSG("null node handle"); + return RMW_RET_ERROR; + } + if (rmw_check_zero_rmw_string_array(node_names) != RMW_RET_OK) { + return RMW_RET_ERROR; + } + if (rmw_check_zero_rmw_string_array(node_namespaces) != RMW_RET_OK) { + return RMW_RET_ERROR; + } + if (rmw_check_zero_rmw_string_array(context_names) != RMW_RET_OK) { + return RMW_RET_ERROR; + } + if (node->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto common_context = static_cast(node->context->impl->common); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + return common_context->graph_cache.get_node_names( + node_names, + node_namespaces, + context_names, &allocator); } } // namespace rmw_fastrtps_shared_cpp From 100a47fa24de20bb928c08a27b2ee23b22e2f6e5 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 13 Mar 2020 15:10:39 -0300 Subject: [PATCH 27/37] Delete unused function Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/CMakeLists.txt | 1 - rmw_fastrtps_cpp/src/rmw_security.cpp | 24 ------------------- rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 1 - rmw_fastrtps_dynamic_cpp/src/rmw_security.cpp | 24 ------------------- 4 files changed, 50 deletions(-) delete mode 100644 rmw_fastrtps_cpp/src/rmw_security.cpp delete mode 100644 rmw_fastrtps_dynamic_cpp/src/rmw_security.cpp diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 9fed15b68..4a443505e 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -74,7 +74,6 @@ add_library(rmw_fastrtps_cpp src/rmw_publisher.cpp src/rmw_request.cpp src/rmw_response.cpp - src/rmw_security.cpp src/rmw_serialize.cpp src/rmw_service.cpp src/rmw_service_names_and_types.cpp diff --git a/rmw_fastrtps_cpp/src/rmw_security.cpp b/rmw_fastrtps_cpp/src/rmw_security.cpp deleted file mode 100644 index 6774438dd..000000000 --- a/rmw_fastrtps_cpp/src/rmw_security.cpp +++ /dev/null @@ -1,24 +0,0 @@ -// 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 "rmw/security.h" - -extern "C" -{ -bool -rmw_use_node_name_in_security_directory_lookup() -{ - return false; -} -} // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index feba67d61..a47720b93 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -81,7 +81,6 @@ add_library(rmw_fastrtps_dynamic_cpp src/rmw_publisher.cpp src/rmw_request.cpp src/rmw_response.cpp - src/rmw_security.cpp src/rmw_serialize.cpp src/rmw_service.cpp src/rmw_service_names_and_types.cpp diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_security.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_security.cpp deleted file mode 100644 index 6774438dd..000000000 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_security.cpp +++ /dev/null @@ -1,24 +0,0 @@ -// 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 "rmw/security.h" - -extern "C" -{ -bool -rmw_use_node_name_in_security_directory_lookup() -{ - return false; -} -} // extern "C" From 7fce106420e93d35cfcc67c59cabb0b9da4fd727 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 17 Mar 2020 10:54:11 -0300 Subject: [PATCH 28/37] Naming: replace context_name with security_context Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp | 4 ++-- rmw_fastrtps_cpp/src/rmw_node_names.cpp | 8 ++++---- .../src/init_rmw_context_impl.cpp | 4 ++-- rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp | 8 ++++---- .../custom_participant_info.hpp | 6 +++--- .../rmw_fastrtps_shared_cpp/participant.hpp | 2 +- .../include/rmw_fastrtps_shared_cpp/rmw_common.hpp | 4 ++-- rmw_fastrtps_shared_cpp/src/participant.cpp | 8 ++++---- rmw_fastrtps_shared_cpp/src/rmw_init.cpp | 14 +++++++------- rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp | 8 ++++---- 10 files changed, 33 insertions(+), 33 deletions(-) diff --git a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp index daf6b3b09..8c9193433 100644 --- a/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -63,7 +63,7 @@ init_context_impl(rmw_context_t * context) context->options.domain_id, &context->options.security_options, (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, - context->options.name, + context->options.security_context, common_context.get()), [&](CustomParticipantInfo * participant_info) { if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { @@ -173,7 +173,7 @@ init_context_impl(rmw_context_t * context) } common_context->graph_cache.add_participant( common_context->gid, - context->options.name); + context->options.security_context); graph_guard_condition.release(); publisher.release(); diff --git a/rmw_fastrtps_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_cpp/src/rmw_node_names.cpp index 6ea7a875d..78833a667 100644 --- a/rmw_fastrtps_cpp/src/rmw_node_names.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node_names.cpp @@ -41,13 +41,13 @@ rmw_get_node_names( } rmw_ret_t -rmw_get_node_names_with_context_names( +rmw_get_node_names_with_security_contexts( const rmw_node_t * node, rcutils_string_array_t * node_names, rcutils_string_array_t * node_namespaces, - rcutils_string_array_t * context_names) + rcutils_string_array_t * security_contexts) { - return rmw_fastrtps_shared_cpp::__rmw_get_node_names_with_context_names( - eprosima_fastrtps_identifier, node, node_names, node_namespaces, context_names); + return rmw_fastrtps_shared_cpp::__rmw_get_node_names_with_security_contexts( + eprosima_fastrtps_identifier, node, node_names, node_namespaces, security_contexts); } } // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp index b712fad12..5fa76772b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -63,7 +63,7 @@ init_context_impl(rmw_context_t * context) context->options.domain_id, &context->options.security_options, (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, - context->options.name, + context->options.security_context, common_context.get()), [&](CustomParticipantInfo * participant_info) { if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { @@ -174,7 +174,7 @@ init_context_impl(rmw_context_t * context) } common_context->graph_cache.add_participant( common_context->gid, - context->options.name); + context->options.security_context); graph_guard_condition.release(); publisher.release(); diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp index b5f1099c5..624fdf641 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp @@ -41,13 +41,13 @@ rmw_get_node_names( } rmw_ret_t -rmw_get_node_names_with_context_names( +rmw_get_node_names_with_security_contexts( const rmw_node_t * node, rcutils_string_array_t * node_names, rcutils_string_array_t * node_namespaces, - rcutils_string_array_t * context_names) + rcutils_string_array_t * security_contexts) { - return rmw_fastrtps_shared_cpp::__rmw_get_node_names_with_context_names( - eprosima_fastrtps_identifier, node, node_names, node_namespaces, context_names); + return rmw_fastrtps_shared_cpp::__rmw_get_node_names_with_security_contexts( + eprosima_fastrtps_identifier, node, node_names, node_namespaces, security_contexts); } } // extern "C" diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index e96b0f65f..3eb79d3c7 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -73,18 +73,18 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DISCOVERED_PARTICIPANT: { auto map = rmw::impl::cpp::parse_key_value(info.info.m_userData); - auto name_found = map.find("contextname"); + auto name_found = map.find("security_context"); if (name_found == map.end()) { return; } - auto context_name = + auto security_context = std::string(name_found->second.begin(), name_found->second.end()); context->graph_cache.add_participant( rmw_fastrtps_shared_cpp::create_rmw_gid( identifier_, info.info.m_guid), - context_name); + security_context); break; } case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT: diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp index 18f8ba658..7fdff205f 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp @@ -30,7 +30,7 @@ create_participant( size_t domain_id, const rmw_security_options_t * security_options, bool localhost_only, - const char * context_name, + const char * security_context, rmw_dds_common::Context * common_context); rmw_ret_t diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp index ecd541012..118d7a075 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -126,12 +126,12 @@ __rmw_init_event( RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t -__rmw_get_node_names_with_context_names( +__rmw_get_node_names_with_security_contexts( const char * identifier, const rmw_node_t * node, rcutils_string_array_t * node_names, rcutils_string_array_t * node_namespaces, - rcutils_string_array_t * context_names); + rcutils_string_array_t * security_contexts); RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index f6b822f7a..145ef583a 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -140,7 +140,7 @@ rmw_fastrtps_shared_cpp::create_participant( size_t domain_id, const rmw_security_options_t * security_options, bool localhost_only, - const char * context_name, + const char * security_context, rmw_dds_common::Context * common_context) { if (!security_options) { @@ -167,16 +167,16 @@ rmw_fastrtps_shared_cpp::create_participant( participantAttrs.rtps.builtin.domainId = static_cast(domain_id); - size_t length = strlen(context_name) + strlen("contextname=;") + 1; + size_t length = strlen(security_context) + strlen("securitycontext=;") + 1; participantAttrs.rtps.userData.resize(length); int written = snprintf( reinterpret_cast(participantAttrs.rtps.userData.data()), - length, "contextname=%s;", context_name); + length, "securitycontext=%s;", security_context); if (written < 0 || written > static_cast(length) - 1) { RMW_SET_ERROR_MSG("failed to populate user_data buffer"); return nullptr; } - participantAttrs.rtps.setName(context_name); + participantAttrs.rtps.setName(security_context); bool leave_middleware_default_qos = false; const char * env_value; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_init.cpp b/rmw_fastrtps_shared_cpp/src/rmw_init.cpp index 6b783d0cd..9bd8c5666 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_init.cpp @@ -39,7 +39,7 @@ rmw_init_options_init( init_options->implementation_identifier = identifier; init_options->allocator = allocator; init_options->impl = nullptr; - init_options->name = NULL; + init_options->security_context = NULL; init_options->domain_id = RMW_DEFAULT_DOMAIN_ID; init_options->security_options = rmw_get_default_security_options(); init_options->localhost_only = RMW_LOCALHOST_ONLY_DEFAULT; @@ -64,19 +64,19 @@ rmw_init_options_copy( const rcutils_allocator_t * allocator = &src->allocator; rmw_ret_t ret = RMW_RET_OK; - allocator->deallocate(dst->name, allocator->state); + allocator->deallocate(dst->security_context, allocator->state); *dst = *src; - dst->name = NULL; + dst->security_context = NULL; dst->security_options = rmw_get_zero_initialized_security_options(); - dst->name = rcutils_strdup(src->name, *allocator); - if (src->name && !dst->name) { + dst->security_context = rcutils_strdup(src->security_context, *allocator); + if (src->security_context && !dst->security_context) { ret = RMW_RET_BAD_ALLOC; goto fail; } return rmw_security_options_copy(&src->security_options, allocator, &dst->security_options); fail: - allocator->deallocate(dst->name, allocator->state); + allocator->deallocate(dst->security_context, allocator->state); return ret; } @@ -91,7 +91,7 @@ rmw_init_options_fini(const char * identifier, rmw_init_options_t * init_options init_options->implementation_identifier, identifier, return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - allocator.deallocate(init_options->name, allocator.state); + allocator.deallocate(init_options->security_context, allocator.state); rmw_security_options_fini(&init_options->security_options, &allocator); *init_options = rmw_get_zero_initialized_init_options(); return RMW_RET_OK; diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp index 4e0d8e07c..d8dd03b48 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_node_names.cpp @@ -69,12 +69,12 @@ __rmw_get_node_names( } rmw_ret_t -__rmw_get_node_names_with_context_names( +__rmw_get_node_names_with_security_contexts( const char * identifier, const rmw_node_t * node, rcutils_string_array_t * node_names, rcutils_string_array_t * node_namespaces, - rcutils_string_array_t * context_names) + rcutils_string_array_t * security_contexts) { if (!node) { RMW_SET_ERROR_MSG("null node handle"); @@ -86,7 +86,7 @@ __rmw_get_node_names_with_context_names( if (rmw_check_zero_rmw_string_array(node_namespaces) != RMW_RET_OK) { return RMW_RET_ERROR; } - if (rmw_check_zero_rmw_string_array(context_names) != RMW_RET_OK) { + if (rmw_check_zero_rmw_string_array(security_contexts) != RMW_RET_OK) { return RMW_RET_ERROR; } if (node->implementation_identifier != identifier) { @@ -99,7 +99,7 @@ __rmw_get_node_names_with_context_names( return common_context->graph_cache.get_node_names( node_names, node_namespaces, - context_names, + security_contexts, &allocator); } } // namespace rmw_fastrtps_shared_cpp From e74a5e0a92c9ca7112edfc10e0f8d1333f6737d3 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Tue, 17 Mar 2020 15:30:09 -0300 Subject: [PATCH 29/37] Don't terminate everything when an error happens in the ros discovery info listener thread. Just log and stop the thread Signed-off-by: Ivan Santiago Paunovic --- .../src/listener_thread.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/listener_thread.cpp b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp index b3c38b533..8df2bb70b 100644 --- a/rmw_fastrtps_shared_cpp/src/listener_thread.cpp +++ b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp @@ -100,13 +100,14 @@ rmw_fastrtps_shared_cpp::join_listener_thread(rmw_context_t * context) return RMW_RET_OK; } -#define TERMINATE(msg) \ - do { \ +#define TERMINATE_THREAD(msg) \ + { \ RCUTILS_SAFE_FWRITE_TO_STDERR( \ RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ - RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) ": terminating ..."); \ - std::terminate(); \ - } while (0) + RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) \ + ": ros discovery info listener thread will shutdown ...\n"); \ + break; \ + } void node_listener(rmw_context_t * context) @@ -130,7 +131,7 @@ node_listener(rmw_context_t * context) rmw_wait_set_t * wait_set = rmw_fastrtps_shared_cpp::__rmw_create_wait_set( context->implementation_identifier, context, 2); if (nullptr == wait_set) { - TERMINATE("failed to create wait set"); + TERMINATE_THREAD("failed to create wait set"); } if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_wait( &subscriptions, @@ -141,7 +142,7 @@ node_listener(rmw_context_t * context) wait_set, nullptr)) { - TERMINATE("rmw_wait failed"); + TERMINATE_THREAD("rmw_wait failed"); } if (subscriptions_buffer[0]) { rmw_dds_common::msg::ParticipantEntitiesInfo msg; @@ -153,7 +154,7 @@ node_listener(rmw_context_t * context) &taken, nullptr)) { - TERMINATE("__rmw_take failed"); + TERMINATE_THREAD("__rmw_take failed"); } if (taken) { if (std::memcmp( @@ -170,7 +171,7 @@ node_listener(rmw_context_t * context) if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( context->implementation_identifier, wait_set)) { - TERMINATE("failed to destroy wait set"); + TERMINATE_THREAD("failed to destroy wait set"); } } } From f10cc19644cb1763d7b9db2d8cecc42e6f3a4051 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 20 Mar 2020 14:44:31 -0300 Subject: [PATCH 30/37] Update error Signed-off-by: Ivan Santiago Paunovic --- .../include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp index 3eb79d3c7..44c8a31ff 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -73,7 +73,7 @@ class ParticipantListener : public eprosima::fastrtps::ParticipantListener case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DISCOVERED_PARTICIPANT: { auto map = rmw::impl::cpp::parse_key_value(info.info.m_userData); - auto name_found = map.find("security_context"); + auto name_found = map.find("securitycontext"); if (name_found == map.end()) { return; From a94234cc623d0f0d2c4321411029297be14f03a4 Mon Sep 17 00:00:00 2001 From: Ivan S Paunovic Date: Tue, 24 Mar 2020 18:54:26 +0000 Subject: [PATCH 31/37] Fix linker failures on Windows Signed-off-by: Ivan S Paunovic --- .../rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp | 3 +++ .../include/rmw_fastrtps_shared_cpp/listener_thread.hpp | 4 ++++ .../include/rmw_fastrtps_shared_cpp/participant.hpp | 3 +++ .../include/rmw_fastrtps_shared_cpp/publisher.hpp | 2 ++ .../include/rmw_fastrtps_shared_cpp/rmw_init.hpp | 5 +++++ .../include/rmw_fastrtps_shared_cpp/subscription.hpp | 2 ++ 6 files changed, 19 insertions(+) diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp index c4dfe3881..13ca78dd8 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp @@ -18,6 +18,8 @@ #include "rmw/init.h" #include "rmw/types.h" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + namespace rmw_fastrtps_shared_cpp { @@ -25,6 +27,7 @@ namespace rmw_fastrtps_shared_cpp /** * Function that should be called when destroying a node. */ +RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t decrement_context_impl_ref_count(rmw_context_t * context); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/listener_thread.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/listener_thread.hpp index 7b9eb6eeb..a4537c4f3 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/listener_thread.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/listener_thread.hpp @@ -17,12 +17,16 @@ #include "rmw/init.h" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + namespace rmw_fastrtps_shared_cpp { +RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t run_listener_thread(rmw_context_t * context); +RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t join_listener_thread(rmw_context_t * context); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp index 7fdff205f..d44f2fb94 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp @@ -20,10 +20,12 @@ #include "rmw_dds_common/context.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" namespace rmw_fastrtps_shared_cpp { +RMW_FASTRTPS_SHARED_CPP_PUBLIC CustomParticipantInfo * create_participant( const char * identifier, @@ -33,6 +35,7 @@ create_participant( const char * security_context, rmw_dds_common::Context * common_context); +RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t destroy_participant(CustomParticipantInfo * info); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/publisher.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/publisher.hpp index 3dae78b6c..fa40362cb 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/publisher.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/publisher.hpp @@ -17,10 +17,12 @@ #include "rmw/rmw.h" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" namespace rmw_fastrtps_shared_cpp { +RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t destroy_publisher( const char * identifier, diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_init.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_init.hpp index 8a17d2426..ce878fa05 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_init.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_init.hpp @@ -19,17 +19,22 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw/init_options.h" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + namespace rmw_fastrtps_shared_cpp { +RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t rmw_init_options_init( const char * identifier, rmw_init_options_t * init_options, rcutils_allocator_t allocator); +RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t rmw_init_options_copy( const char * identifier, const rmw_init_options_t * src, rmw_init_options_t * dst); +RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t rmw_init_options_fini(const char * identifier, rmw_init_options_t * init_options); diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/subscription.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/subscription.hpp index b959a3669..523b916b3 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/subscription.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/subscription.hpp @@ -17,10 +17,12 @@ #include "rmw/rmw.h" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" namespace rmw_fastrtps_shared_cpp { +RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t destroy_subscription( const char * identifier, From f043aaa8eeec2471fa7a16e9dc3f143d77eca0ff Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 26 Mar 2020 13:33:55 -0300 Subject: [PATCH 32/37] Address peer review comments Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_shared_cpp/src/participant.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 145ef583a..0a7d51fe4 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -167,8 +167,7 @@ rmw_fastrtps_shared_cpp::create_participant( participantAttrs.rtps.builtin.domainId = static_cast(domain_id); - size_t length = strlen(security_context) + strlen("securitycontext=;") + 1; - participantAttrs.rtps.userData.resize(length); + size_t length = snprintf(nullptr, 0, "securitycontext=%s;", security_context); int written = snprintf( reinterpret_cast(participantAttrs.rtps.userData.data()), length, "securitycontext=%s;", security_context); From fa959e1d8c387e657b127e5ae93de5eac4202a31 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Thu, 26 Mar 2020 14:35:43 -0300 Subject: [PATCH 33/37] Fix bug Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_shared_cpp/src/participant.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rmw_fastrtps_shared_cpp/src/participant.cpp b/rmw_fastrtps_shared_cpp/src/participant.cpp index 0a7d51fe4..2f6cad6ae 100644 --- a/rmw_fastrtps_shared_cpp/src/participant.cpp +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -167,7 +167,8 @@ rmw_fastrtps_shared_cpp::create_participant( participantAttrs.rtps.builtin.domainId = static_cast(domain_id); - size_t length = snprintf(nullptr, 0, "securitycontext=%s;", security_context); + size_t length = snprintf(nullptr, 0, "securitycontext=%s;", security_context) + 1; + participantAttrs.rtps.userData.resize(length); int written = snprintf( reinterpret_cast(participantAttrs.rtps.userData.data()), length, "securitycontext=%s;", security_context); From 0bf81122039925b1fac3690a0d0b4494d5734de4 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Fri, 27 Mar 2020 14:03:55 -0300 Subject: [PATCH 34/37] Use static_cast instead of reinterpret_cast Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/publisher.cpp | 2 +- rmw_fastrtps_dynamic_cpp/src/publisher.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/rmw_fastrtps_cpp/src/publisher.cpp b/rmw_fastrtps_cpp/src/publisher.cpp index 4f5905a2c..c83ed5c89 100644 --- a/rmw_fastrtps_cpp/src/publisher.cpp +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -156,7 +156,7 @@ rmw_fastrtps_cpp::create_publisher( } rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; rmw_publisher->data = info; - rmw_publisher->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + rmw_publisher->topic_name = static_cast(rmw_allocate(strlen(topic_name) + 1)); if (!rmw_publisher->topic_name) { RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); diff --git a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp index 20fa3bceb..6e5a6ad2c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -177,7 +177,7 @@ rmw_fastrtps_dynamic_cpp::create_publisher( rmw_publisher->can_loan_messages = false; rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; rmw_publisher->data = info; - rmw_publisher->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + rmw_publisher->topic_name = static_cast(rmw_allocate(strlen(topic_name) + 1)); if (!rmw_publisher->topic_name) { RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); From 0fa080ea46e073f5a5216eae930f80b239ec947d Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 30 Mar 2020 09:37:56 -0300 Subject: [PATCH 35/37] Remove unused variable Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp index 48e381f76..4d95448dd 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp @@ -45,8 +45,6 @@ namespace rmw_fastrtps_shared_cpp { -constexpr char kLoggerTag[] = "rmw_fastrtps_shared_cpp"; - /** * Validate the input data of node_info_and_types functions. * From 47abe69f76aa53906fd33ef49312ae31f7bb5630 Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Mon, 30 Mar 2020 17:08:57 -0300 Subject: [PATCH 36/37] Fix UB Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/src/rmw_node.cpp | 3 ++- rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp | 3 ++- rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp | 2 +- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/rmw_fastrtps_cpp/src/rmw_node.cpp b/rmw_fastrtps_cpp/src/rmw_node.cpp index bad5bebda..6b38efe66 100644 --- a/rmw_fastrtps_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node.cpp @@ -72,12 +72,13 @@ rmw_create_node( rmw_ret_t rmw_destroy_node(rmw_node_t * node) { + rmw_context_t * context = node->context; rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_node( eprosima_fastrtps_identifier, node); if (RMW_RET_OK != ret) { return ret; } - return rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(node->context); + return rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(context); } rmw_ret_t diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp index 2a0a629e2..110a32c0b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp @@ -72,12 +72,13 @@ rmw_create_node( rmw_ret_t rmw_destroy_node(rmw_node_t * node) { + rmw_context_t * context = node->context; rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_node( eprosima_fastrtps_identifier, node); if (RMW_RET_OK != ret) { return ret; } - return rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(node->context); + return rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(context); } rmw_ret_t diff --git a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp index 22c19a315..58332a061 100644 --- a/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp +++ b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp @@ -89,7 +89,6 @@ rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(rmw_context_t * contex } else if (RMW_RET_OK != ret) { RMW_SET_ERROR_MSG(error_string.str); } - delete common_context; common_context->graph_cache.clear_on_change_callback(); if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( @@ -100,6 +99,7 @@ rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(rmw_context_t * contex "couldn't destroy graph_guard_condtion"); } + delete common_context; context->impl->common = nullptr; context->impl->participant_info = nullptr; } From 8b6f5567d659c869ffa30bc43df66661333b8b1b Mon Sep 17 00:00:00 2001 From: Ivan Santiago Paunovic Date: Wed, 1 Apr 2020 09:27:40 -0300 Subject: [PATCH 37/37] Only link about the typesupport we need Signed-off-by: Ivan Santiago Paunovic --- rmw_fastrtps_cpp/CMakeLists.txt | 4 ++++ rmw_fastrtps_dynamic_cpp/CMakeLists.txt | 4 ++++ 2 files changed, 8 insertions(+) diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 4a443505e..c643581cc 100644 --- a/rmw_fastrtps_cpp/CMakeLists.txt +++ b/rmw_fastrtps_cpp/CMakeLists.txt @@ -105,6 +105,10 @@ ament_target_dependencies(rmw_fastrtps_cpp "rosidl_generator_c" ) +target_link_libraries(rmw_fastrtps_cpp + ${rmw_dds_common_LIBRARIES__rosidl_typesupport_fastrtps_cpp} +) + configure_rmw_library(rmw_fastrtps_cpp) # Causes the visibility macros to use dllexport rather than dllimport, diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index a47720b93..1c0ffe6c3 100644 --- a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt +++ b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt @@ -114,6 +114,10 @@ ament_target_dependencies(rmw_fastrtps_dynamic_cpp "rosidl_generator_c" ) +target_link_libraries(rmw_fastrtps_dynamic_cpp + ${rmw_dds_common_LIBRARIES__rosidl_typesupport_introspection_cpp} +) + configure_rmw_library(rmw_fastrtps_dynamic_cpp) # Causes the visibility macros to use dllexport rather than dllimport,