diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 45314ad1a..c5a3f8c9c 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 @@ -70,6 +74,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 @@ -81,7 +86,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) @@ -91,6 +100,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" @@ -121,6 +131,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..434094454 --- /dev/null +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -0,0 +1,180 @@ +// 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 ec81fa301..07a34dfbf 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; @@ -126,7 +129,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; } @@ -136,7 +139,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; @@ -202,15 +205,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_); } @@ -222,7 +264,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 dba510c25..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,18 +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/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" { @@ -76,165 +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; - } + 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); - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); + if (!publisher) { return nullptr; } - Participant * participant = impl->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } + auto common_context = static_cast(node->context->impl->common); - 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; - const eprosima::fastrtps::rtps::GUID_t * guid = nullptr; - - 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, 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); - 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) { - 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 1226c2cef..923e90995 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; @@ -140,7 +143,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( @@ -208,16 +210,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 17ec03807..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,136 +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..b886261f9 --- /dev/null +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -0,0 +1,171 @@ +// 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 c64f4215b..793d58ccc 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 @@ -67,6 +73,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 @@ -74,6 +81,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 @@ -84,8 +92,9 @@ add_library(rmw_fastrtps_dynamic_cpp src/rmw_trigger_guard_condition.cpp src/rmw_wait.cpp src/rmw_wait_set.cpp - src/type_support_common.cpp src/serialization_format.cpp + src/subscription.cpp + src/type_support_common.cpp ) target_link_libraries(rmw_fastrtps_dynamic_cpp fastcdr fastrtps) @@ -98,6 +107,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..6c4011853 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -0,0 +1,198 @@ +// 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" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; + +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; + } + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_support->data; + + 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_ = _create_message_type_support(type_support->data, + info->typesupport_identifier_); + _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; + } + 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 f2bf46dc2..39554fbf0 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" @@ -67,13 +68,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; @@ -133,7 +136,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; } @@ -143,7 +146,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; @@ -209,15 +212,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_); } @@ -229,7 +271,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 50093dc96..c0309cffc 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -20,14 +20,14 @@ #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_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/publisher.hpp" -#include "type_support_common.hpp" +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; @@ -76,161 +76,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; - } + auto impl = static_cast(node->context->impl->participant_info); - 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; - } + rmw_publisher_t * publisher = rmw_fastrtps_dynamic_cpp::create_publisher( + impl, + type_supports, + topic_name, + qos_policies, + publisher_options, + false, + true); - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); + if (!publisher) { 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; - } - } + auto common_context = static_cast(node->context->impl->common); - 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; - } - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support->data; - - 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_ = _create_message_type_support(type_support->data, - info->typesupport_identifier_); - _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); - 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) { - 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_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 8a04615e7..e1cbf420e 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" @@ -79,7 +80,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; @@ -215,16 +218,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 b07f98e99..1c74dd625 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,15 +21,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_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/subscription.hpp" #include "type_support_common.hpp" @@ -80,137 +76,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); + auto participant_info = + static_cast(node->context->impl->participant_info); - info = new (std::nothrow) CustomSubscriberInfo(); - if (!info) { - RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); + 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_support->data; - 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_ = _create_message_type_support(type_support->data, - info->typesupport_identifier_); - _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_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp new file mode 100644 index 000000000..1a36812a2 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -0,0 +1,176 @@ +// 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" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; + + +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; + } + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_support->data; + + 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_ = _create_message_type_support(type_support->data, + info->typesupport_identifier_); + _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; + } + 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 a8ba7c2f5..9d06fa5fe 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 @@ -75,6 +79,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 ) @@ -87,6 +92,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..48ae2a221 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,27 @@ 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 +103,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 +140,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 a73ce8fbf..567abde87 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 @@ -143,7 +141,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 @@ -261,7 +259,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/include/rmw_fastrtps_shared_cpp/topic_cache.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp deleted file mode 100644 index a6201a3fb..000000000 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/topic_cache.hpp +++ /dev/null @@ -1,307 +0,0 @@ -// Copyright 2018 Amazon.com, Inc. or its affiliates. All Rights Reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RMW_FASTRTPS_SHARED_CPP__TOPIC_CACHE_HPP_ -#define RMW_FASTRTPS_SHARED_CPP__TOPIC_CACHE_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "fastrtps/participant/Participant.h" -#include "fastrtps/rtps/common/Guid.h" -#include "fastrtps/rtps/common/InstanceHandle.h" -#include "rcpputils/thread_safety_annotations.hpp" -#include "rcutils/logging_macros.h" - -#include "qos.hpp" - -typedef eprosima::fastrtps::rtps::GUID_t GUID_t; - -/** - * A data structure that encapsulates all the data associated with a publisher - * or subscription by the topic it publishes or subscribes to - */ -struct TopicData -{ - GUID_t participant_guid; - GUID_t entity_guid; - std::string topic_type; - rmw_qos_profile_t qos_profile; -}; - -/** - * Topic cache data structure. Manages relationships between participants and topics. - */ -class TopicCache -{ -private: - using TopicToTypes = std::unordered_map>; - using ParticipantTopicMap = std::map; - using TopicNameToTopicData = std::unordered_map>; - - /** - * Map of topic names to TopicData. Where topic data is vector of tuples containing - * participant GUID, topic type and the qos policy of the respective participant. - * Topics here are represented as one to many, DDS XTypes 1.2 - * specifies application code that 'generally' uses a 1-1 relationship. - * However, generic services such as logger and monitor, can discover - * multiple types on the same topic. - */ - TopicNameToTopicData topic_name_to_topic_data_; - - /** - * Map of participant GUIDS to a set of topic-type. - */ - ParticipantTopicMap participant_to_topics_; - - /** - * Helper function to initialize an empty TopicData for a topic name. - * - * \param topic_name the topic name for which the TopicNameToTopicData map should be initialized. - * \param topic_name_to_topic_data the map to initialize. - */ - void initializeTopicDataMap( - const std::string & topic_name, - TopicNameToTopicData & topic_name_to_topic_data) - { - if (topic_name_to_topic_data.find(topic_name) == topic_name_to_topic_data.end()) { - topic_name_to_topic_data[topic_name] = std::vector(); - } - } - - /** - * Helper function to initialize a topic vector. - * - * \param topic_name the topic name for which the TopicToTypes map should be initialized. - * \param topic_to_types the map to initialize. - */ - void initializeTopicTypesMap(const std::string & topic_name, TopicToTypes & topic_to_types) - { - if (topic_to_types.find(topic_name) == topic_to_types.end()) { - topic_to_types[topic_name] = std::vector(); - } - } - - /** - * Helper function to initialize the set inside a participant map. - * - * \param map - * \param guid - */ - void initializeParticipantMap( - ParticipantTopicMap & map, - GUID_t guid) - { - if (map.find(guid) == map.end()) { - map[guid] = TopicToTypes(); - } - } - -public: - /** - * \return a map of topic name to the vector of topic types used. - */ - const TopicToTypes getTopicToTypes() const - { - TopicToTypes topic_to_types; - for (const auto & it : topic_name_to_topic_data_) { - topic_to_types[it.first] = std::vector(); - for (const auto & topic_data : it.second) { - topic_to_types[it.first].push_back(topic_data.topic_type); - } - } - return topic_to_types; - } - - /** - * \return a map of participant guid to the vector of topic names used. - */ - const ParticipantTopicMap & getParticipantToTopics() const - { - return participant_to_topics_; - } - - /** - * \return a map of topic name to a vector of GUID_t, type name and qos profile tuple. - */ - const TopicNameToTopicData & getTopicNameToTopicData() const - { - return topic_name_to_topic_data_; - } - - /** - * Add a topic based on discovery. - * - * \param rtpsParticipantKey the participant key of the discovered publisher or subscription - * \param entity_guid the guid of the publisher or subscription - * \param topic_name the topic name associated with the discovered publisher or subscription - * \param type_name the topic type associated with the discovered publisher or subscription - * \param dds_qos the dds qos policy of the discovered publisher or subscription - * \return true if a change has been recorded - */ - template - bool addTopic( - const eprosima::fastrtps::rtps::InstanceHandle_t & rtpsParticipantKey, - const GUID_t & entity_guid, - const std::string & topic_name, - const std::string & type_name, - const T & dds_qos) - { - initializeTopicDataMap(topic_name, topic_name_to_topic_data_); - auto participant_guid = iHandle2GUID(rtpsParticipantKey); - initializeParticipantMap(participant_to_topics_, participant_guid); - initializeTopicTypesMap(topic_name, participant_to_topics_[entity_guid]); - if (rcutils_logging_logger_is_enabled_for("rmw_fastrtps_shared_cpp", - RCUTILS_LOG_SEVERITY_DEBUG)) - { - std::stringstream guid_stream; - guid_stream << entity_guid; - RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_shared_cpp", - "Adding topic '%s' with type '%s' for node '%s'", - topic_name.c_str(), type_name.c_str(), guid_stream.str().c_str()); - } - rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown; - dds_qos_to_rmw_qos(dds_qos, &qos_profile); - TopicData topic_data = { - participant_guid, - entity_guid, - type_name, - qos_profile - }; - topic_name_to_topic_data_[topic_name].push_back(topic_data); - participant_to_topics_[participant_guid][topic_name].push_back(type_name); - return true; - } - - /** - * Remove a topic based on discovery. - * - * \param rtpsParticipantKey the participant key of the publisher or subscription - * \param entity_guid the guid of the publisher or subscription - * \param topic_name the topic name associated with the publisher or subscription - * \param type_name the topic type associated with the publisher or subscription - * \return true if a change has been recorded - */ - bool removeTopic( - const eprosima::fastrtps::rtps::InstanceHandle_t & rtpsParticipantKey, - const eprosima::fastrtps::rtps::GUID_t & entity_guid, - const std::string & topic_name, - const std::string & type_name) - { - if (topic_name_to_topic_data_.find(topic_name) == topic_name_to_topic_data_.end()) { - RCUTILS_LOG_DEBUG_NAMED( - "rmw_fastrtps_shared_cpp", - "unexpected removal on topic '%s' with type '%s'", - topic_name.c_str(), type_name.c_str()); - return false; - } - { - auto & type_vec = topic_name_to_topic_data_[topic_name]; - type_vec.erase(std::find_if(type_vec.begin(), type_vec.end(), - [type_name, entity_guid](const auto & topic_data) { - return type_name.compare(topic_data.topic_type) == 0 && - entity_guid == topic_data.entity_guid; - })); - if (type_vec.empty()) { - topic_name_to_topic_data_.erase(topic_name); - } - } - auto participant_guid = iHandle2GUID(rtpsParticipantKey); - auto guid_topics_pair = participant_to_topics_.find(participant_guid); - if (guid_topics_pair != participant_to_topics_.end() && - guid_topics_pair->second.find(topic_name) != guid_topics_pair->second.end()) - { - auto & type_vec = guid_topics_pair->second[topic_name]; - type_vec.erase(std::find(type_vec.begin(), type_vec.end(), type_name)); - if (type_vec.empty()) { - participant_to_topics_[participant_guid].erase(topic_name); - } - if (participant_to_topics_[participant_guid].empty()) { - participant_to_topics_.erase(participant_guid); - } - } else { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_shared_cpp", - "Unable to remove topic, does not exist '%s' with type '%s'", - topic_name.c_str(), type_name.c_str()); - } - return true; - } -}; - -inline std::ostream & operator<<( - std::ostream & ostream, - const TopicCache & topic_cache) -{ - std::stringstream map_ss; - map_ss << "Participant Info: " << std::endl; - for (auto & elem : topic_cache.getParticipantToTopics()) { - std::ostringstream stream; - stream << " Topics: " << std::endl; - for (auto & types : elem.second) { - stream << " " << types.first << ": "; - std::copy(types.second.begin(), types.second.end(), - std::ostream_iterator(stream, ",")); - stream << std::endl; - } - map_ss << elem.first << std::endl << stream.str(); - } - std::stringstream topics_ss; - topics_ss << "Cumulative TopicToTypes: " << std::endl; - for (auto & elem : topic_cache.getTopicToTypes()) { - std::ostringstream stream; - std::copy(elem.second.begin(), elem.second.end(), std::ostream_iterator(stream, - ",")); - topics_ss << " " << elem.first << " : " << stream.str() << std::endl; - } - ostream << map_ss.str() << topics_ss.str(); - return ostream; -} - -template -class LockedObject -{ -private: - mutable std::mutex mutex_; - T object_ RCPPUTILS_TSA_GUARDED_BY(mutex_); - -public: - /** - * \return a reference to this object to lock. - */ - std::mutex & getMutex() const RCPPUTILS_TSA_RETURN_CAPABILITY(mutex_) - { - return mutex_; - } - - T & operator()() - { - return object_; - } - - const T & operator()() const - { - return object_; - } -}; - -#endif // RMW_FASTRTPS_SHARED_CPP__TOPIC_CACHE_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..4b0cc5e5d --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp @@ -0,0 +1,34 @@ +// 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" + +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." + ); + memset(rmw_gid.data, 0, RMW_GID_STORAGE_SIZE); + memcpy(rmw_gid.data, &guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); + return rmw_gid; +} diff --git a/rmw_fastrtps_shared_cpp/src/demangle.cpp b/rmw_fastrtps_shared_cpp/src/demangle.cpp index b7a6aba1d..4f50b84fc 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,59 +55,63 @@ _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", "service topic has prefix but no suffix" ", 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 "". @@ -150,3 +157,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..599916ea3 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -0,0 +1,238 @@ +// 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 547fa1a2a..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,52 +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 @@ -94,51 +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 eaa2846de..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 @@ -23,10 +23,14 @@ #include "rmw/topic_endpoint_info_array.h" #include "rmw/topic_endpoint_info.h" -#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/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" namespace rmw_fastrtps_shared_cpp { @@ -73,245 +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]; - memset(&rmw_gid, 0, RMW_GID_STORAGE_SIZE); - memcpy(&rmw_gid, &topic_data.entity_guid, sizeof(eprosima::fastrtps::rtps::GUID_t)); - 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); - } - participants_info->count = count; + 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); } @@ -324,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 6ff864883..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,176 +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, @@ -350,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 @@ -384,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 37e70e9d9..ddc97fb67 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,67 +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. * @@ -140,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, @@ -330,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(identifier, node, allocator, node_name, @@ -339,41 +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); - - __log_debug_information(*impl); + auto common_context = static_cast(node->context->impl->common); - 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, @@ -382,124 +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); + 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_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 @@ -511,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 @@ -530,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 c5b6bf031..000000000 --- a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp +++ /dev/null @@ -1,265 +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); -}