diff --git a/rmw_fastrtps_cpp/CMakeLists.txt b/rmw_fastrtps_cpp/CMakeLists.txt index 497953dcf..c643581cc 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,8 @@ add_library(rmw_fastrtps_cpp src/get_service.cpp src/get_subscriber.cpp src/identifier.cpp + src/init_rmw_context_impl.cpp + src/publisher.cpp src/rmw_logging.cpp src/rmw_client.cpp src/rmw_compare_gids_equal.cpp @@ -82,7 +85,11 @@ add_library(rmw_fastrtps_cpp src/rmw_wait.cpp src/rmw_wait_set.cpp src/serialization_format.cpp + src/subscription.cpp src/type_support_common.cpp + src/gid__type_support.cpp + src/node_entities_info__type_support.cpp + src/participant_entities_info__type_support.cpp ) target_link_libraries(rmw_fastrtps_cpp fastcdr fastrtps) @@ -92,11 +99,16 @@ 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" ) +target_link_libraries(rmw_fastrtps_cpp + ${rmw_dds_common_LIBRARIES__rosidl_typesupport_fastrtps_cpp} +) + configure_rmw_library(rmw_fastrtps_cpp) # Causes the visibility macros to use dllexport rather than dllimport, @@ -122,6 +134,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/init_rmw_context_impl.hpp b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp new file mode 100644 index 000000000..d9e2724e3 --- /dev/null +++ b/rmw_fastrtps_cpp/include/rmw_fastrtps_cpp/init_rmw_context_impl.hpp @@ -0,0 +1,33 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ +#define RMW_FASTRTPS_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ + +#include "rmw/init.h" +#include "rmw/types.h" + +namespace rmw_fastrtps_cpp +{ + +/// Increment `rmw_context_impl_t` reference count, initializing it if necessary. +/** + * Should be called when creating a node, and before using `context->impl`. + */ +rmw_ret_t +increment_context_impl_ref_count(rmw_context_t * context); + +} // namespace rmw_fastrtps_cpp + +#endif // RMW_FASTRTPS_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ diff --git a/rmw_fastrtps_cpp/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/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..1ace143fe --- /dev/null +++ b/rmw_fastrtps_cpp/src/gid__type_support.cpp @@ -0,0 +1,56 @@ +// 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" + +// This file is needed and can't be generated by `rosidl_typesupport_cpp`, +// because adding a dependency to that package generates a cyclic dependency +// (`rosidl_typesupport_cpp` exports a dependency on `rmw_implementation`). + +namespace rosidl_typesupport_cpp +{ + +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/init_rmw_context_impl.cpp b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp new file mode 100644 index 000000000..8c9193433 --- /dev/null +++ b/rmw_fastrtps_cpp/src/init_rmw_context_impl.cpp @@ -0,0 +1,202 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw_fastrtps_cpp/init_rmw_context_impl.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 "rmw_fastrtps_shared_cpp/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, + context->options.security_context, + common_context.get()), + [&](CustomParticipantInfo * participant_info) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { + RCUTILS_SAFE_FWRITE_TO_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 = true; + 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(), + "ros_discovery_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)) + { + RCUTILS_SAFE_FWRITE_TO_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(), + "ros_discovery_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)) + { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy subscription after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!subscription) { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + graph_guard_condition( + rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), + [&](rmw_guard_condition_t * p) { + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); + if (ret != RMW_RET_OK) { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy guard condition after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!graph_guard_condition) { + return RMW_RET_BAD_ALLOC; + } + + common_context->graph_cache.set_on_change_callback( + [guard_condition = graph_guard_condition.get()]() { + rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + eprosima_fastrtps_identifier, + guard_condition); + }); + + common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, participant_info->participant->getGuid()); + common_context->pub = publisher.get(); + common_context->sub = subscription.get(); + common_context->graph_guard_condition = graph_guard_condition.get(); + + context->impl->common = common_context.get(); + context->impl->participant_info = participant_info.get(); + + rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); + if (RMW_RET_OK != ret) { + return ret; + } + common_context->graph_cache.add_participant( + common_context->gid, + context->options.security_context); + + graph_guard_condition.release(); + publisher.release(); + subscription.release(); + common_context.release(); + participant_info.release(); + return RMW_RET_OK; +} + +rmw_ret_t +rmw_fastrtps_cpp::increment_context_impl_ref_count(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; +} 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..b69a648b7 --- /dev/null +++ b/rmw_fastrtps_cpp/src/node_entities_info__type_support.cpp @@ -0,0 +1,56 @@ +// 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" + +// This file is needed and can't be generated by `rosidl_typesupport_cpp`, +// because adding a dependency to that package generates a cyclic dependency +// (`rosidl_typesupport_cpp` exports a dependency on `rmw_implementation`). + +namespace rosidl_typesupport_cpp +{ + +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..492581b73 --- /dev/null +++ b/rmw_fastrtps_cpp/src/participant_entities_info__type_support.cpp @@ -0,0 +1,56 @@ +// 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" + +// This file is needed and can't be generated by `rosidl_typesupport_cpp`, +// because adding a dependency to that package generates a cyclic dependency +// (`rosidl_typesupport_cpp` exports a dependency on `rmw_implementation`). + +namespace rosidl_typesupport_cpp +{ + +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..c83ed5c89 --- /dev/null +++ b/rmw_fastrtps_cpp/src/publisher.cpp @@ -0,0 +1,181 @@ +// 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); + + 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 = static_cast(rmw_allocate(strlen(topic_name) + 1)); + + if (!rmw_publisher->topic_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); + 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/rmw_client.cpp b/rmw_fastrtps_cpp/src/rmw_client.cpp index c8a1a85d1..6d2d53456 100644 --- a/rmw_fastrtps_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_cpp/src/rmw_client.cpp @@ -25,6 +25,7 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" @@ -62,13 +63,15 @@ rmw_create_client( return nullptr; } - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + if (!participant_info) { + RMW_SET_ERROR_MSG("participant info is null"); return nullptr; } - Participant * participant = impl->participant; + Participant * participant = participant_info->participant; if (!participant) { RMW_SET_ERROR_MSG("participant handle is null"); return nullptr; @@ -130,7 +133,7 @@ rmw_create_client( _register_type(participant, info->response_type_support_); } - if (!impl->leave_middleware_default_qos) { + if (!participant_info->leave_middleware_default_qos) { subscriberParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } @@ -140,7 +143,7 @@ rmw_create_client( subscriberParam.topic.topicName = _create_topic_name( qos_policies, ros_service_response_prefix, service_name, "Reply"); - if (!impl->leave_middleware_default_qos) { + if (!participant_info->leave_middleware_default_qos) { publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; publisherParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; @@ -206,15 +209,54 @@ rmw_create_client( } memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_publisher_->getGuid()); + common_context->graph_cache.associate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( + gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + goto fail; + } + } + return rmw_client; fail: if (info != nullptr) { if (info->request_publisher_ != nullptr) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_publisher_->getGuid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removePublisher(info->request_publisher_); } if (info->response_subscriber_ != nullptr) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); + common_context->graph_cache.dissociate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removeSubscriber(info->response_subscriber_); } @@ -226,7 +268,7 @@ rmw_create_client( delete info->listener_; } - if (impl) { + if (participant_info) { if (info->request_type_support_ != nullptr) { rmw_fastrtps_shared_cpp::_unregister_type(participant, info->request_type_support_); } diff --git a/rmw_fastrtps_cpp/src/rmw_init.cpp b/rmw_fastrtps_cpp/src/rmw_init.cpp index 4334babea..5cd5927f3 100644 --- a/rmw_fastrtps_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_cpp/src/rmw_init.cpp @@ -1,3 +1,4 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -12,64 +13,69 @@ // 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/rmw_init.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" extern "C" { rmw_ret_t rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t allocator) { - RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); - if (NULL != init_options->implementation_identifier) { - RMW_SET_ERROR_MSG("expected zero-initialized init_options"); - return RMW_RET_INVALID_ARGUMENT; - } - init_options->instance_id = 0; - init_options->implementation_identifier = eprosima_fastrtps_identifier; - init_options->allocator = allocator; - init_options->impl = nullptr; - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::rmw_init_options_init( + eprosima_fastrtps_identifier, init_options, allocator); } rmw_ret_t rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) { - RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - src, - src->implementation_identifier, - eprosima_fastrtps_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (NULL != dst->implementation_identifier) { - RMW_SET_ERROR_MSG("expected zero-initialized dst"); - return RMW_RET_INVALID_ARGUMENT; - } - *dst = *src; - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::rmw_init_options_copy( + eprosima_fastrtps_identifier, src, dst); } rmw_ret_t rmw_init_options_fini(rmw_init_options_t * init_options) { - RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR(&(init_options->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); - *init_options = rmw_get_zero_initialized_init_options(); - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::rmw_init_options_fini( + eprosima_fastrtps_identifier, init_options); } +using rmw_dds_common::msg::ParticipantEntitiesInfo; + 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 +85,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 (std::nothrow) rmw_context_impl_t()); + if (!context_impl) { + RMW_SET_ERROR_MSG("failed to allocate context impl"); + return RMW_RET_BAD_ALLOC; + } + context->options = rmw_get_zero_initialized_init_options(); + ret = rmw_init_options_copy(options, &context->options); + if (RMW_RET_OK != ret) { + if (RMW_RET_OK != rmw_init_options_fini(&context->options)) { + RMW_SAFE_FWRITE_TO_STDERR( + "'rmw_init_options_fini' failed while being executed due to '" + RCUTILS_STRINGIFY(__function__) "' failing.\n"); + } + return ret; + } + context->impl = context_impl.release(); + clean_when_fail.release(); return RMW_RET_OK; } @@ -106,8 +129,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..6b38efe66 100644 --- a/rmw_fastrtps_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node.cpp @@ -1,3 +1,4 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -18,16 +19,18 @@ #include #include "rcutils/filesystem.h" -#include "rcutils/logging_macros.h" #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/init_rmw_context_impl.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" +#include "rmw_fastrtps_cpp/init_rmw_context_impl.hpp" extern "C" { @@ -37,25 +40,45 @@ rmw_create_node( const char * name, const char * namespace_, size_t domain_id, - const rmw_node_security_options_t * security_options, bool localhost_only) { + (void)domain_id; + (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 rmw_fastrtps_shared_cpp::__rmw_create_node( - eprosima_fastrtps_identifier, name, namespace_, domain_id, security_options, localhost_only); + return nullptr); + + if (RMW_RET_OK != rmw_fastrtps_cpp::increment_context_impl_ref_count(context)) { + return nullptr; + } + + rmw_node_t * node = rmw_fastrtps_shared_cpp::__rmw_create_node( + context, eprosima_fastrtps_identifier, name, namespace_); + + if (nullptr == node) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(context)) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "'decrement_context_impl_ref_count' failed while being executed due to '" + RCUTILS_STRINGIFY(__function__) "' failing"); + } + } + return node; } rmw_ret_t rmw_destroy_node(rmw_node_t * node) { - return rmw_fastrtps_shared_cpp::__rmw_destroy_node( + rmw_context_t * context = node->context; + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_node( eprosima_fastrtps_identifier, node); + if (RMW_RET_OK != ret) { + return ret; + } + return rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(context); } rmw_ret_t diff --git a/rmw_fastrtps_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_cpp/src/rmw_node_names.cpp index bcdbe6648..78833a667 100644 --- a/rmw_fastrtps_cpp/src/rmw_node_names.cpp +++ b/rmw_fastrtps_cpp/src/rmw_node_names.cpp @@ -39,4 +39,15 @@ rmw_get_node_names( return rmw_fastrtps_shared_cpp::__rmw_get_node_names( eprosima_fastrtps_identifier, node, node_names, node_namespaces); } + +rmw_ret_t +rmw_get_node_names_with_security_contexts( + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * security_contexts) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_node_names_with_security_contexts( + eprosima_fastrtps_identifier, node, node_names, node_namespaces, security_contexts); +} } // extern "C" diff --git a/rmw_fastrtps_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_cpp/src/rmw_publisher.cpp index 66c1b7799..45557ec51 100644 --- a/rmw_fastrtps_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_cpp/src/rmw_publisher.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -20,19 +21,14 @@ #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" -#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" -#include "rmw_fastrtps_shared_cpp/names.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" -#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" +#include "rmw_fastrtps_cpp/publisher.hpp" -#include "./type_support_common.hpp" - -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" extern "C" { @@ -77,163 +73,40 @@ rmw_create_publisher( return nullptr; } - if (!topic_name || strlen(topic_name) == 0) { - RMW_SET_ERROR_MSG("publisher topic is null or empty string"); - return nullptr; - } - - if (!qos_policies) { - RMW_SET_ERROR_MSG("qos_policies is null"); - return nullptr; - } - - if (!publisher_options) { - RMW_SET_ERROR_MSG("publisher_options is null"); - return nullptr; - } - - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return nullptr; - } - - Participant * participant = impl->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } - - const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); - if (!type_support) { - type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); - if (!type_support) { - RMW_SET_ERROR_MSG("type support not from this implementation"); - return nullptr; - } - } - - CustomPublisherInfo * info = nullptr; - rmw_publisher_t * rmw_publisher = nullptr; - eprosima::fastrtps::PublisherAttributes publisherParam; + rmw_publisher_t * publisher = rmw_fastrtps_cpp::create_publisher( + static_cast(node->context->impl->participant_info), + type_supports, + topic_name, + qos_policies, + publisher_options, + false, + true); - if (!is_valid_qos(*qos_policies)) { + if (!publisher) { return nullptr; } - // Load default XML profile. - Domain::getDefaultPublisherAttributes(publisherParam); + auto common_context = static_cast(node->context->impl->common); - // TODO(karsten1987) Verify consequences for std::unique_ptr? - info = new (std::nothrow) CustomPublisherInfo(); - if (!info) { - RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); - return nullptr; - } - - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support->data; - - auto callbacks = static_cast(type_support->data); - std::string type_name = _create_type_name(callbacks); - if ( - !Domain::getRegisteredType( - participant, type_name.c_str(), - reinterpret_cast(&info->type_support_))) + auto info = static_cast(publisher->data); { - info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("Failed to allocate MessageTypeSupport"); - goto fail; - } - _register_type(participant, info->type_support_); - } - - if (!impl->leave_middleware_default_qos) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - } - - publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - publisherParam.topic.topicDataType = type_name; - publisherParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); - - // 1 Heartbeat every 10ms - // publisherParam.times.heartbeatPeriod.seconds = 0; - // publisherParam.times.heartbeatPeriod.fraction = 42949673; - - // 300000 bytes each 10ms - // ThroughputControllerDescriptor throughputController{3000000, 10}; - // publisherParam.throughputController = throughputController; - - if (!get_datawriter_qos(*qos_policies, publisherParam)) { - RMW_SET_ERROR_MSG("failed to get datawriter qos"); - goto fail; - } - - info->listener_ = new (std::nothrow) PubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); - goto fail; - } - - info->publisher_ = Domain::createPublisher(participant, publisherParam, info->listener_); - if (!info->publisher_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); - goto fail; - } - - info->publisher_gid.implementation_identifier = eprosima_fastrtps_identifier; - static_assert( - sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, - "RMW_GID_STORAGE_SIZE insufficient to store the rmw_fastrtps_cpp GID implementation." - ); - - memset(info->publisher_gid.data, 0, RMW_GID_STORAGE_SIZE); - rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( - info->publisher_->getGuid(), - info->publisher_gid.data); - - rmw_publisher = rmw_publisher_allocate(); - if (!rmw_publisher) { - RMW_SET_ERROR_MSG("failed to allocate publisher"); - goto fail; - } - rmw_publisher->can_loan_messages = false; - rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; - rmw_publisher->data = info; - rmw_publisher->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); - - if (!rmw_publisher->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); - goto fail; - } - - memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); - - rmw_publisher->options = *publisher_options; - - return rmw_publisher; - -fail: - if (info) { - if (info->type_support_ != nullptr) { - delete info->type_support_; - } - if (info->listener_ != nullptr) { - delete info->listener_; + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_writer( + info->publisher_gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + rmw_fastrtps_shared_cpp::__rmw_destroy_publisher( + eprosima_fastrtps_identifier, node, publisher); + return nullptr; } - delete info; } - - if (rmw_publisher) { - rmw_publisher_free(rmw_publisher); - } - - return nullptr; + return publisher; } rmw_ret_t diff --git a/rmw_fastrtps_cpp/src/rmw_service.cpp b/rmw_fastrtps_cpp/src/rmw_service.cpp index 70722d497..e1dbe4eac 100644 --- a/rmw_fastrtps_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_cpp/src/rmw_service.cpp @@ -36,10 +36,11 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" -#include "./type_support_common.hpp" +#include "type_support_common.hpp" using Domain = eprosima::fastrtps::Domain; using Participant = eprosima::fastrtps::Participant; @@ -74,7 +75,9 @@ rmw_create_service( return nullptr; } - const CustomParticipantInfo * impl = static_cast(node->data); + const CustomParticipantInfo * impl = + static_cast(node->context->impl->participant_info); + auto common_context = static_cast(node->context->impl->common); if (!impl) { RMW_SET_ERROR_MSG("node impl is null"); return nullptr; @@ -144,7 +147,6 @@ rmw_create_service( subscriberParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } - subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; subscriberParam.topic.topicDataType = request_type_name; subscriberParam.topic.topicName = _create_topic_name( @@ -212,16 +214,55 @@ rmw_create_service( } memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_subscriber_->getGuid()); + common_context->graph_cache.associate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_writer( + gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + goto fail; + } + } + return rmw_service; fail: if (info) { if (info->response_publisher_) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removePublisher(info->response_publisher_); } if (info->request_subscriber_) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_subscriber_->getGuid()); + common_context->graph_cache.dissociate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removeSubscriber(info->request_subscriber_); } diff --git a/rmw_fastrtps_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_cpp/src/rmw_subscription.cpp index 590a77c85..3a1d884ee 100644 --- a/rmw_fastrtps_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_cpp/src/rmw_subscription.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -12,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include #include "rmw/allocators.h" #include "rmw/error_handling.h" @@ -21,21 +22,11 @@ #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" -#include "rmw_fastrtps_shared_cpp/names.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" -#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" - -#include "fastrtps/participant/Participant.h" -#include "fastrtps/subscriber/Subscriber.h" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_cpp/identifier.hpp" - -#include "./type_support_common.hpp" - -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; +#include "rmw_fastrtps_cpp/subscription.hpp" extern "C" { @@ -80,138 +71,40 @@ rmw_create_subscription( return nullptr; } - if (!topic_name || strlen(topic_name) == 0) { - RMW_SET_ERROR_MSG("subscription topic is null or empty string"); - return nullptr; - } - - if (!qos_policies) { - RMW_SET_ERROR_MSG("qos_profile is null"); - return nullptr; - } - - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return nullptr; - } - - Participant * participant = impl->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } - - const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_C); - if (!type_support) { - type_support = get_message_typesupport_handle( - type_supports, RMW_FASTRTPS_CPP_TYPESUPPORT_CPP); - if (!type_support) { - RMW_SET_ERROR_MSG("type support not from this implementation"); - return nullptr; - } - } - - if (!is_valid_qos(*qos_policies)) { - return nullptr; - } - - CustomSubscriberInfo * info = nullptr; - rmw_subscription_t * rmw_subscription = nullptr; - eprosima::fastrtps::SubscriberAttributes subscriberParam; - - // Load default XML profile. - Domain::getDefaultSubscriberAttributes(subscriberParam); - - info = new (std::nothrow) CustomSubscriberInfo(); - if (!info) { - RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); + auto participant_info = + static_cast(node->context->impl->participant_info); + rmw_subscription_t * subscription = rmw_fastrtps_cpp::create_subscription( + participant_info, + type_supports, + topic_name, + qos_policies, + subscription_options, + false, // use no keyed topic + true); // create subscription listener + if (!subscription) { return nullptr; } - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_support->data; - - auto callbacks = static_cast(type_support->data); - std::string type_name = _create_type_name(callbacks); - if ( - !Domain::getRegisteredType( - participant, type_name.c_str(), - reinterpret_cast(&info->type_support_))) + auto common_context = static_cast(node->context->impl->common); + auto info = static_cast(subscription->data); { - info->type_support_ = new (std::nothrow) MessageTypeSupport_cpp(callbacks); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("failed to allocate MessageTypeSupport_cpp"); - goto fail; - } - _register_type(participant, info->type_support_); - } - - if (!impl->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - } - - subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - subscriberParam.topic.topicDataType = type_name; - subscriberParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); - - if (!get_datareader_qos(*qos_policies, subscriberParam)) { - RMW_SET_ERROR_MSG("failed to get datareader qos"); - goto fail; - } - - info->listener_ = new (std::nothrow) SubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); - goto fail; - } - - info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); - if (!info->subscriber_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); - goto fail; - } - - rmw_subscription = rmw_subscription_allocate(); - if (!rmw_subscription) { - RMW_SET_ERROR_MSG("failed to allocate subscription"); - goto fail; - } - rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; - rmw_subscription->data = info; - rmw_subscription->topic_name = - reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); - - if (!rmw_subscription->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); - goto fail; - } - - memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); - - rmw_subscription->options = *subscription_options; - rmw_subscription->can_loan_messages = false; - return rmw_subscription; - -fail: - - if (info != nullptr) { - if (info->type_support_ != nullptr) { - delete info->type_support_; - } - if (info->listener_ != nullptr) { - delete info->listener_; + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( + info->subscription_gid_, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( + eprosima_fastrtps_identifier, node, subscription); + return nullptr; } - delete info; } - - if (rmw_subscription) { - rmw_subscription_free(rmw_subscription); - } - - return nullptr; + return subscription; } rmw_ret_t diff --git a/rmw_fastrtps_cpp/src/subscription.cpp b/rmw_fastrtps_cpp/src/subscription.cpp new file mode 100644 index 000000000..6e1122404 --- /dev/null +++ b/rmw_fastrtps_cpp/src/subscription.cpp @@ -0,0 +1,174 @@ +// 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 "rcutils/allocator.h" +#include "rcutils/strdup.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_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 = rcutils_strdup(topic_name, rcutils_get_default_allocator()); + if (!rmw_subscription->topic_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); + goto fail; + } + + rmw_subscription->options = *subscription_options; + return rmw_subscription; + +fail: + if (info != nullptr) { + delete info->type_support_; + delete info->listener_; + delete info; + } + rmw_subscription_free(rmw_subscription); + return nullptr; +} +} // namespace rmw_fastrtps_cpp diff --git a/rmw_fastrtps_cpp/test/test_gid_message.cpp b/rmw_fastrtps_cpp/test/test_gid_message.cpp new file mode 100644 index 000000000..f6f30c53a --- /dev/null +++ b/rmw_fastrtps_cpp/test/test_gid_message.cpp @@ -0,0 +1,42 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rmw_dds_common/msg/gid.hpp" + +#include "rosidl_generator_c/message_type_support_struct.h" +#include "rosidl_typesupport_cpp/message_type_support.hpp" +#include "rosidl_typesupport_fastrtps_cpp/identifier.hpp" + +using rmw_dds_common::msg::Gid; + + +TEST(Test_gid, constructor_destructor) { + Gid gid; + ASSERT_EQ(24u, gid.data.size()); + gid.data[0] = 'a'; +} + +TEST(Test_gid, get_typesupport) { + const rosidl_message_type_support_t * type_support_1 = + rosidl_typesupport_cpp::get_message_type_support_handle(); + const rosidl_message_type_support_t * type_support_2 = get_message_typesupport_handle( + type_support_1, rosidl_typesupport_fastrtps_cpp::typesupport_identifier); + + ASSERT_EQ(type_support_1->typesupport_identifier, type_support_2->typesupport_identifier); + ASSERT_EQ(type_support_1->data, type_support_2->data); + ASSERT_EQ(type_support_1->func, type_support_2->func); +} diff --git a/rmw_fastrtps_dynamic_cpp/CMakeLists.txt b/rmw_fastrtps_dynamic_cpp/CMakeLists.txt index 7c5305851..1c0ffe6c3 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,12 @@ 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/init_rmw_context_impl.cpp + src/node_entities_info__type_support.cpp + src/participant_entities_info__type_support.cpp + src/publisher.cpp src/rmw_client.cpp src/rmw_compare_gids_equal.cpp src/rmw_count.cpp @@ -68,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 @@ -85,10 +91,11 @@ add_library(rmw_fastrtps_dynamic_cpp src/rmw_trigger_guard_condition.cpp src/rmw_wait.cpp src/rmw_wait_set.cpp + src/serialization_format.cpp + src/subscription.cpp src/type_support_common.cpp src/type_support_proxy.cpp src/type_support_registry.cpp - src/serialization_format.cpp ) target_link_libraries(rmw_fastrtps_dynamic_cpp fastcdr fastrtps) @@ -101,11 +108,16 @@ 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" ) +target_link_libraries(rmw_fastrtps_dynamic_cpp + ${rmw_dds_common_LIBRARIES__rosidl_typesupport_introspection_cpp} +) + configure_rmw_library(rmw_fastrtps_dynamic_cpp) # Causes the visibility macros to use dllexport rather than dllimport, diff --git a/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp new file mode 100644 index 000000000..11cc71da4 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp @@ -0,0 +1,33 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ + +#include "rmw/init.h" +#include "rmw/types.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +/// Increment `rmw_context_impl_t` reference count, initializing it if necessary. +/** + * Should be called when creating a node, and before using `context->impl`. + */ +rmw_ret_t +increment_context_impl_ref_count(rmw_context_t * context); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ diff --git a/rmw_fastrtps_dynamic_cpp/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/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..84c59cd28 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/gid__type_support.cpp @@ -0,0 +1,56 @@ +// 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" + +// This file is needed and can't be generated by `rosidl_typesupport_cpp`, +// because adding a dependency to that package generates a cyclic dependency +// (`rosidl_typesupport_cpp` exports a dependency on `rmw_implementation`). + +namespace rosidl_typesupport_cpp +{ + +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/init_rmw_context_impl.cpp b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp new file mode 100644 index 000000000..5fa76772b --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/init_rmw_context_impl.cpp @@ -0,0 +1,203 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.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 "rmw_fastrtps_shared_cpp/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, + context->options.security_context, + common_context.get()), + [&](CustomParticipantInfo * participant_info) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::destroy_participant(participant_info)) { + RCUTILS_SAFE_FWRITE_TO_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 = true; + 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(), + "ros_discovery_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)) + { + RCUTILS_SAFE_FWRITE_TO_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(), + "ros_discovery_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)) + { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy subscription after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!subscription) { + return RMW_RET_BAD_ALLOC; + } + + std::unique_ptr> + graph_guard_condition( + rmw_fastrtps_shared_cpp::__rmw_create_guard_condition(eprosima_fastrtps_identifier), + [&](rmw_guard_condition_t * p) { + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition(p); + if (ret != RMW_RET_OK) { + RMW_SAFE_FWRITE_TO_STDERR( + "Failed to destroy guard condition after function: '" + RCUTILS_STRINGIFY(__function__) "' failed.\n"); + } + }); + if (!graph_guard_condition) { + return RMW_RET_BAD_ALLOC; + } + + common_context->graph_cache.set_on_change_callback( + [guard_condition = graph_guard_condition.get()]() { + rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + eprosima_fastrtps_identifier, + guard_condition); + }); + + + common_context->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, participant_info->participant->getGuid()); + common_context->pub = publisher.get(); + common_context->sub = subscription.get(); + common_context->graph_guard_condition = graph_guard_condition.get(); + + context->impl->common = common_context.get(); + context->impl->participant_info = participant_info.get(); + + rmw_ret_t ret = rmw_fastrtps_shared_cpp::run_listener_thread(context); + if (RMW_RET_OK != ret) { + return ret; + } + common_context->graph_cache.add_participant( + common_context->gid, + context->options.security_context); + + graph_guard_condition.release(); + publisher.release(); + subscription.release(); + common_context.release(); + participant_info.release(); + return RMW_RET_OK; +} + +rmw_ret_t +rmw_fastrtps_dynamic_cpp::increment_context_impl_ref_count(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; +} 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..370935018 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/node_entities_info__type_support.cpp @@ -0,0 +1,56 @@ +// 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" + +// This file is needed and can't be generated by `rosidl_typesupport_cpp`, +// because adding a dependency to that package generates a cyclic dependency +// (`rosidl_typesupport_cpp` exports a dependency on `rmw_implementation`). + +namespace rosidl_typesupport_cpp +{ + +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..151d79744 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/participant_entities_info__type_support.cpp @@ -0,0 +1,56 @@ +// 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" + +// This file is needed and can't be generated by `rosidl_typesupport_cpp`, +// because adding a dependency to that package generates a cyclic dependency +// (`rosidl_typesupport_cpp` exports a dependency on `rmw_implementation`). + +namespace rosidl_typesupport_cpp +{ + +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..6e5a6ad2c --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/publisher.cpp @@ -0,0 +1,203 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/publisher.hpp" + +#include "type_support_common.hpp" +#include "type_support_registry.hpp" + +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; +using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; + +rmw_publisher_t * +rmw_fastrtps_dynamic_cpp::create_publisher( + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_publisher_options_t * publisher_options, + bool keyed, + bool create_publisher_listener) +{ + (void)keyed; + (void)create_publisher_listener; + + if (!participant_info) { + RMW_SET_ERROR_MSG("participant_info is null"); + return nullptr; + } + + if (!topic_name || strlen(topic_name) == 0) { + RMW_SET_ERROR_MSG("publisher topic is null or empty string"); + return nullptr; + } + + if (!qos_policies) { + RMW_SET_ERROR_MSG("qos_policies is null"); + return nullptr; + } + + if (!publisher_options) { + RMW_SET_ERROR_MSG("publisher_options is null"); + return nullptr; + } + + Participant * participant = participant_info->participant; + if (!participant) { + RMW_SET_ERROR_MSG("participant handle is null"); + return nullptr; + } + + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return nullptr; + } + } + + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + + CustomPublisherInfo * info = nullptr; + rmw_publisher_t * rmw_publisher = nullptr; + eprosima::fastrtps::PublisherAttributes publisherParam; + + // Load default XML profile. + Domain::getDefaultPublisherAttributes(publisherParam); + + info = new (std::nothrow) CustomPublisherInfo(); + if (!info) { + RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); + return nullptr; + } + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto type_impl = type_registry.get_message_type_support(type_support); + if (!type_impl) { + delete info; + RMW_SET_ERROR_MSG("failed to allocate type support"); + return nullptr; + } + + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_impl; + + std::string type_name = _create_type_name( + type_support->data, info->typesupport_identifier_); + if ( + !Domain::getRegisteredType( + participant, type_name.c_str(), + reinterpret_cast(&info->type_support_))) + { + info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); + if (!info->type_support_) { + RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); + goto fail; + } + _register_type(participant, info->type_support_); + } + + if (!participant_info->leave_middleware_default_qos) { + publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + publisherParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; + publisherParam.topic.topicDataType = type_name; + publisherParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); + + // 1 Heartbeat every 10ms + // publisherParam.times.heartbeatPeriod.seconds = 0; + // publisherParam.times.heartbeatPeriod.fraction = 42949673; + + // 300000 bytes each 10ms + // ThroughputControllerDescriptor throughputController{3000000, 10}; + // publisherParam.throughputController = throughputController; + + if (!get_datawriter_qos(*qos_policies, publisherParam)) { + RMW_SET_ERROR_MSG("failed to get datawriter qos"); + goto fail; + } + + info->listener_ = new (std::nothrow) PubListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); + goto fail; + } + + info->publisher_ = Domain::createPublisher(participant, publisherParam, info->listener_); + if (!info->publisher_) { + RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); + goto fail; + } + + info->publisher_gid = 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->can_loan_messages = false; + rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; + rmw_publisher->data = info; + rmw_publisher->topic_name = static_cast(rmw_allocate(strlen(topic_name) + 1)); + + if (!rmw_publisher->topic_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); + goto fail; + } + + memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); + + rmw_publisher->options = *publisher_options; + + return rmw_publisher; + +fail: + if (info) { + delete info->type_support_; + delete info->listener_; + delete info; + } + type_registry.return_message_type_support(type_support); + rmw_publisher_free(rmw_publisher); + + return nullptr; +} diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp index 44823c8a8..3a815e027 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp @@ -29,6 +29,7 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" @@ -70,13 +71,15 @@ rmw_create_client( return nullptr; } - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + if (!participant_info) { + RMW_SET_ERROR_MSG("participant info is null"); return nullptr; } - Participant * participant = impl->participant; + Participant * participant = participant_info->participant; if (!participant) { RMW_SET_ERROR_MSG("participant handle is null"); return nullptr; @@ -160,7 +163,7 @@ rmw_create_client( _register_type(participant, info->response_type_support_); } - if (!impl->leave_middleware_default_qos) { + if (!participant_info->leave_middleware_default_qos) { subscriberParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; } @@ -170,7 +173,7 @@ rmw_create_client( subscriberParam.topic.topicName = _create_topic_name( qos_policies, ros_service_response_prefix, service_name, "Reply"); - if (!impl->leave_middleware_default_qos) { + if (!participant_info->leave_middleware_default_qos) { publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; publisherParam.historyMemoryPolicy = eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; @@ -236,15 +239,54 @@ rmw_create_client( } memcpy(const_cast(rmw_client->service_name), service_name, strlen(service_name) + 1); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_publisher_->getGuid()); + common_context->graph_cache.associate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( + gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + goto fail; + } + } + return rmw_client; fail: if (info != nullptr) { if (info->request_publisher_ != nullptr) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_publisher_->getGuid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removePublisher(info->request_publisher_); } if (info->response_subscriber_ != nullptr) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_subscriber_->getGuid()); + common_context->graph_cache.dissociate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removeSubscriber(info->response_subscriber_); } @@ -256,7 +298,7 @@ rmw_create_client( delete info->listener_; } - if (impl) { + if (participant_info) { if (info->request_type_support_ != nullptr) { rmw_fastrtps_shared_cpp::_unregister_type(participant, info->request_type_support_); } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp index 3e57dfbb8..9d320cfc1 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_init.cpp @@ -1,3 +1,4 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -12,64 +13,69 @@ // 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/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/rmw_init.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" extern "C" { rmw_ret_t rmw_init_options_init(rmw_init_options_t * init_options, rcutils_allocator_t allocator) { - RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); - if (NULL != init_options->implementation_identifier) { - RMW_SET_ERROR_MSG("expected zero-initialized init_options"); - return RMW_RET_INVALID_ARGUMENT; - } - init_options->instance_id = 0; - init_options->implementation_identifier = eprosima_fastrtps_identifier; - init_options->allocator = allocator; - init_options->impl = nullptr; - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::rmw_init_options_init( + eprosima_fastrtps_identifier, init_options, allocator); } rmw_ret_t rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst) { - RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT); - RMW_CHECK_TYPE_IDENTIFIERS_MATCH( - src, - src->implementation_identifier, - eprosima_fastrtps_identifier, - return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); - if (NULL != dst->implementation_identifier) { - RMW_SET_ERROR_MSG("expected zero-initialized dst"); - return RMW_RET_INVALID_ARGUMENT; - } - *dst = *src; - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::rmw_init_options_copy( + eprosima_fastrtps_identifier, src, dst); } rmw_ret_t rmw_init_options_fini(rmw_init_options_t * init_options) { - RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); - RCUTILS_CHECK_ALLOCATOR(&(init_options->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); - *init_options = rmw_get_zero_initialized_init_options(); - return RMW_RET_OK; + return rmw_fastrtps_shared_cpp::rmw_init_options_fini( + eprosima_fastrtps_identifier, init_options); } +using rmw_dds_common::msg::ParticipantEntitiesInfo; + 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 +85,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 (std::nothrow) rmw_context_impl_t()); + if (!context_impl) { + RMW_SET_ERROR_MSG("failed to allocate context impl"); + return RMW_RET_BAD_ALLOC; + } + context->options = rmw_get_zero_initialized_init_options(); + ret = rmw_init_options_copy(options, &context->options); + if (RMW_RET_OK != ret) { + if (RMW_RET_OK != rmw_init_options_fini(&context->options)) { + RMW_SAFE_FWRITE_TO_STDERR( + "'rmw_init_options_fini' failed while being executed due to '" + RCUTILS_STRINGIFY(__function__) "' failing.\n"); + } + return ret; + } + context->impl = context_impl.release(); + clean_when_fail.release(); return RMW_RET_OK; } @@ -106,8 +129,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..110a32c0b 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node.cpp @@ -1,3 +1,4 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -25,9 +26,11 @@ #include "rmw/impl/cpp/macros.hpp" #include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp" extern "C" { @@ -37,9 +40,10 @@ rmw_create_node( const char * name, const char * namespace_, size_t domain_id, - const rmw_node_security_options_t * security_options, bool localhost_only) { + (void)domain_id; + (void)localhost_only; RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL); RMW_CHECK_TYPE_IDENTIFIERS_MATCH( init context, @@ -47,15 +51,34 @@ rmw_create_node( eprosima_fastrtps_identifier, // TODO(wjwwood): replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when refactored return NULL); - return rmw_fastrtps_shared_cpp::__rmw_create_node( - eprosima_fastrtps_identifier, name, namespace_, domain_id, security_options, localhost_only); + + if (RMW_RET_OK != rmw_fastrtps_dynamic_cpp::increment_context_impl_ref_count(context)) { + return nullptr; + } + + rmw_node_t * node = rmw_fastrtps_shared_cpp::__rmw_create_node( + context, eprosima_fastrtps_identifier, name, namespace_); + + if (nullptr == node) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(context)) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "'decrement_context_impl_ref_count' failed while being executed due to '" + RCUTILS_STRINGIFY(__function__) "' failing"); + } + } + return node; } rmw_ret_t rmw_destroy_node(rmw_node_t * node) { - return rmw_fastrtps_shared_cpp::__rmw_destroy_node( + rmw_context_t * context = node->context; + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_node( eprosima_fastrtps_identifier, node); + if (RMW_RET_OK != ret) { + return ret; + } + return rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(context); } rmw_ret_t diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp index 55440d33a..624fdf641 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_node_names.cpp @@ -39,4 +39,15 @@ rmw_get_node_names( return rmw_fastrtps_shared_cpp::__rmw_get_node_names( eprosima_fastrtps_identifier, node, node_names, node_namespaces); } + +rmw_ret_t +rmw_get_node_names_with_security_contexts( + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * security_contexts) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_node_names_with_security_contexts( + eprosima_fastrtps_identifier, node, node_names, node_namespaces, security_contexts); +} } // extern "C" diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp index 3ee5c34f2..6dbd0786c 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp @@ -20,23 +20,18 @@ #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" -#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" -#include "rmw_fastrtps_shared_cpp/names.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" -#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/publisher.hpp" + +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" #include "type_support_common.hpp" #include "type_support_registry.hpp" -using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; -using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; - extern "C" { rmw_ret_t @@ -80,172 +75,42 @@ rmw_create_publisher( return nullptr; } - if (!topic_name || strlen(topic_name) == 0) { - RMW_SET_ERROR_MSG("publisher topic is null or empty string"); - return nullptr; - } - - if (!qos_policies) { - RMW_SET_ERROR_MSG("qos_policies is null"); - return nullptr; - } - - if (!publisher_options) { - RMW_SET_ERROR_MSG("publisher_options is null"); - return nullptr; - } - - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return nullptr; - } - - Participant * participant = impl->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } + auto impl = static_cast(node->context->impl->participant_info); - const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); - if (!type_support) { - type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (!type_support) { - RMW_SET_ERROR_MSG("type support not from this implementation"); - return nullptr; - } - } + rmw_publisher_t * publisher = rmw_fastrtps_dynamic_cpp::create_publisher( + impl, + type_supports, + topic_name, + qos_policies, + publisher_options, + false, + true); - if (!is_valid_qos(*qos_policies)) { + if (!publisher) { return nullptr; } - CustomPublisherInfo * info = nullptr; - rmw_publisher_t * rmw_publisher = nullptr; - eprosima::fastrtps::PublisherAttributes publisherParam; - - // Load default XML profile. - Domain::getDefaultPublisherAttributes(publisherParam); + auto common_context = static_cast(node->context->impl->common); - // TODO(karsten1987) Verify consequences for std::unique_ptr? - info = new (std::nothrow) CustomPublisherInfo(); - if (!info) { - RMW_SET_ERROR_MSG("failed to allocate CustomPublisherInfo"); - return nullptr; - } - - TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); - auto type_impl = type_registry.get_message_type_support(type_support); - if (!type_impl) { - delete info; - RMW_SET_ERROR_MSG("failed to allocate type support"); - return nullptr; - } - - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_impl; - - std::string type_name = _create_type_name( - type_support->data, info->typesupport_identifier_); - if (!Domain::getRegisteredType( - participant, type_name.c_str(), - reinterpret_cast(&info->type_support_))) + auto info = static_cast(publisher->data); { - info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); - goto fail; - } - _register_type(participant, info->type_support_); - } - - if (!impl->leave_middleware_default_qos) { - publisherParam.qos.m_publishMode.kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; - publisherParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - } - - publisherParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - publisherParam.topic.topicDataType = type_name; - publisherParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); - - // 1 Heartbeat every 10ms - // publisherParam.times.heartbeatPeriod.seconds = 0; - // publisherParam.times.heartbeatPeriod.fraction = 42949673; - - // 300000 bytes each 10ms - // ThroughputControllerDescriptor throughputController{3000000, 10}; - // publisherParam.throughputController = throughputController; - - if (!get_datawriter_qos(*qos_policies, publisherParam)) { - RMW_SET_ERROR_MSG("failed to get datawriter qos"); - goto fail; - } - - info->listener_ = new (std::nothrow) PubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher listener"); - goto fail; - } - - info->publisher_ = Domain::createPublisher(participant, publisherParam, info->listener_); - if (!info->publisher_) { - RMW_SET_ERROR_MSG("create_publisher() could not create publisher"); - goto fail; - } - - info->publisher_gid.implementation_identifier = eprosima_fastrtps_identifier; - static_assert( - sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, - "RMW_GID_STORAGE_SIZE insufficient to store the rmw_fastrtps_dynamic_cpp GID implementation." - ); - - memset(info->publisher_gid.data, 0, RMW_GID_STORAGE_SIZE); - rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( - info->publisher_->getGuid(), - info->publisher_gid.data); - - rmw_publisher = rmw_publisher_allocate(); - if (!rmw_publisher) { - RMW_SET_ERROR_MSG("failed to allocate publisher"); - goto fail; - } - rmw_publisher->can_loan_messages = false; - rmw_publisher->implementation_identifier = eprosima_fastrtps_identifier; - rmw_publisher->data = info; - rmw_publisher->topic_name = reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); - - if (!rmw_publisher->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for publisher topic name"); - goto fail; - } - - memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); - - rmw_publisher->options = *publisher_options; - - return rmw_publisher; - -fail: - if (info) { - if (info->type_support_ != nullptr) { - delete info->type_support_; - } - if (info->listener_ != nullptr) { - delete info->listener_; + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_writer( + info->publisher_gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + rmw_fastrtps_shared_cpp::__rmw_destroy_publisher( + eprosima_fastrtps_identifier, node, publisher); + return nullptr; } - delete info; - } - - type_registry.return_message_type_support(type_support); - - if (rmw_publisher) { - rmw_publisher_free(rmw_publisher); } - - return nullptr; + return publisher; } rmw_ret_t @@ -300,6 +165,8 @@ rmw_return_loaned_message_from_publisher( return RMW_RET_UNSUPPORTED; } +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; + rmw_ret_t rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) { diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp index 12975bf4c..8f8b5648e 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp @@ -35,6 +35,7 @@ #include "rosidl_typesupport_introspection_cpp/identifier.hpp" #include "rosidl_typesupport_introspection_c/identifier.h" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" #include "rmw_fastrtps_shared_cpp/names.hpp" @@ -81,7 +82,9 @@ rmw_create_service( return nullptr; } - const CustomParticipantInfo * impl = static_cast(node->data); + const CustomParticipantInfo * impl = + static_cast(node->context->impl->participant_info); + auto common_context = static_cast(node->context->impl->common); if (!impl) { RMW_SET_ERROR_MSG("node impl is null"); return nullptr; @@ -241,16 +244,55 @@ rmw_create_service( } memcpy(const_cast(rmw_service->service_name), service_name, strlen(service_name) + 1); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_subscriber_->getGuid()); + common_context->graph_cache.associate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_writer( + gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + goto fail; + } + } + return rmw_service; fail: if (info) { if (info->response_publisher_) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_publisher_->getGuid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removePublisher(info->response_publisher_); } if (info->request_subscriber_) { + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_subscriber_->getGuid()); + common_context->graph_cache.dissociate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); Domain::removeSubscriber(info->request_subscriber_); } diff --git a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp index 594f0c017..67d0583a0 100644 --- a/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp +++ b/rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include #include "rmw/allocators.h" #include "rmw/error_handling.h" @@ -21,25 +21,15 @@ #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" #include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" -#include "rmw_fastrtps_shared_cpp/names.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" -#include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" - -#include "fastrtps/participant/Participant.h" -#include "fastrtps/subscriber/Subscriber.h" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/subscription.hpp" #include "type_support_common.hpp" #include "type_support_registry.hpp" -using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; -using Domain = eprosima::fastrtps::Domain; -using Participant = eprosima::fastrtps::Participant; -using TopicDataType = eprosima::fastrtps::TopicDataType; -using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; - extern "C" { rmw_ret_t @@ -83,152 +73,41 @@ rmw_create_subscription( return nullptr; } - if (!topic_name || strlen(topic_name) == 0) { - RMW_SET_ERROR_MSG("subscription topic is null or empty string"); - return nullptr; - } - - if (!qos_policies) { - RMW_SET_ERROR_MSG("qos_policies is null"); - return nullptr; - } - - if (!subscription_options) { - RMW_SET_ERROR_MSG("subscription_options is null"); - return nullptr; - } - - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return nullptr; - } - - Participant * participant = impl->participant; - if (!participant) { - RMW_SET_ERROR_MSG("participant handle is null"); - return nullptr; - } - - const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_c__identifier); - if (!type_support) { - type_support = get_message_typesupport_handle( - type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); - if (!type_support) { - RMW_SET_ERROR_MSG("type support not from this implementation"); - return nullptr; - } - } - - if (!is_valid_qos(*qos_policies)) { - return nullptr; - } - - CustomSubscriberInfo * info = nullptr; - rmw_subscription_t * rmw_subscription = nullptr; - eprosima::fastrtps::SubscriberAttributes subscriberParam; - - // Load default XML profile. - Domain::getDefaultSubscriberAttributes(subscriberParam); - - info = new (std::nothrow) CustomSubscriberInfo(); - if (!info) { - RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); - return nullptr; - } + auto participant_info = + static_cast(node->context->impl->participant_info); - TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); - auto type_impl = type_registry.get_message_type_support(type_support); - if (!type_impl) { - delete info; - RMW_SET_ERROR_MSG("failed to allocate type support"); + rmw_subscription_t * subscription = rmw_fastrtps_dynamic_cpp::create_subscription( + participant_info, + type_supports, + topic_name, + qos_policies, + subscription_options, + false, + true); + if (!subscription) { return nullptr; } - info->typesupport_identifier_ = type_support->typesupport_identifier; - info->type_support_impl_ = type_impl; - - std::string type_name = _create_type_name( - type_support->data, info->typesupport_identifier_); - if (!Domain::getRegisteredType( - participant, type_name.c_str(), - reinterpret_cast(&info->type_support_))) + auto common_context = static_cast(node->context->impl->common); + auto info = static_cast(subscription->data); { - info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); - if (!info->type_support_) { - RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); - goto fail; - } - _register_type(participant, info->type_support_); - } - - if (!impl->leave_middleware_default_qos) { - subscriberParam.historyMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - } - - subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; - subscriberParam.topic.topicDataType = type_name; - subscriberParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); - - if (!get_datareader_qos(*qos_policies, subscriberParam)) { - RMW_SET_ERROR_MSG("failed to get datareader qos"); - goto fail; - } - - info->listener_ = new (std::nothrow) SubListener(info); - if (!info->listener_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); - goto fail; - } - - info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); - if (!info->subscriber_) { - RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); - goto fail; - } - - rmw_subscription = rmw_subscription_allocate(); - if (!rmw_subscription) { - RMW_SET_ERROR_MSG("failed to allocate subscription"); - goto fail; - } - rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; - rmw_subscription->data = info; - rmw_subscription->topic_name = - reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); - - if (!rmw_subscription->topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); - goto fail; - } - - memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); - - rmw_subscription->options = *subscription_options; - rmw_subscription->can_loan_messages = false; - return rmw_subscription; - -fail: - - if (info != nullptr) { - if (info->type_support_ != nullptr) { - delete info->type_support_; - } - if (info->listener_ != nullptr) { - delete info->listener_; + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( + info->subscription_gid_, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( + eprosima_fastrtps_identifier, node, subscription); + return nullptr; } - delete info; - } - - type_registry.return_message_type_support(type_support); - - if (rmw_subscription) { - rmw_subscription_free(rmw_subscription); } - - return nullptr; + return subscription; } rmw_ret_t @@ -249,6 +128,8 @@ rmw_subscription_get_actual_qos( subscription, qos); } +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; + rmw_ret_t rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) { diff --git a/rmw_fastrtps_dynamic_cpp/src/subscription.cpp b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp new file mode 100644 index 000000000..5ea84f5a3 --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp/src/subscription.cpp @@ -0,0 +1,194 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "fastrtps/participant/Participant.h" +#include "fastrtps/subscriber/Subscriber.h" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_dynamic_cpp/subscription.hpp" + +#include "type_support_common.hpp" +#include "type_support_registry.hpp" + +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; +using TypeSupportProxy = rmw_fastrtps_dynamic_cpp::TypeSupportProxy; + + +namespace rmw_fastrtps_dynamic_cpp +{ + +rmw_subscription_t * +create_subscription( + const CustomParticipantInfo * participant_info, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_subscription_options_t * subscription_options, + bool keyed, + bool create_subscription_listener) +{ + (void)keyed; + (void)create_subscription_listener; + + if (!topic_name || strlen(topic_name) == 0) { + RMW_SET_ERROR_MSG("subscription topic is null or empty string"); + return nullptr; + } + + if (!qos_policies) { + RMW_SET_ERROR_MSG("qos_policies is null"); + return nullptr; + } + + if (!subscription_options) { + RMW_SET_ERROR_MSG("subscription_options is null"); + return nullptr; + } + + Participant * participant = participant_info->participant; + if (!participant) { + RMW_SET_ERROR_MSG("participant handle is null"); + return nullptr; + } + + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return nullptr; + } + } + + if (!is_valid_qos(*qos_policies)) { + return nullptr; + } + + CustomSubscriberInfo * info = nullptr; + rmw_subscription_t * rmw_subscription = nullptr; + eprosima::fastrtps::SubscriberAttributes subscriberParam; + + // Load default XML profile. + Domain::getDefaultSubscriberAttributes(subscriberParam); + + info = new (std::nothrow) CustomSubscriberInfo(); + if (!info) { + RMW_SET_ERROR_MSG("failed to allocate CustomSubscriberInfo"); + return nullptr; + } + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto type_impl = type_registry.get_message_type_support(type_support); + if (!type_impl) { + delete info; + RMW_SET_ERROR_MSG("failed to allocate type support"); + return nullptr; + } + + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_impl; + + std::string type_name = _create_type_name( + type_support->data, info->typesupport_identifier_); + if ( + !Domain::getRegisteredType( + participant, type_name.c_str(), + reinterpret_cast(&info->type_support_))) + { + info->type_support_ = new (std::nothrow) TypeSupportProxy(type_impl); + if (!info->type_support_) { + RMW_SET_ERROR_MSG("failed to allocate TypeSupportProxy"); + goto fail; + } + _register_type(participant, info->type_support_); + } + + if (!participant_info->leave_middleware_default_qos) { + subscriberParam.historyMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + + subscriberParam.topic.topicKind = eprosima::fastrtps::rtps::NO_KEY; + subscriberParam.topic.topicDataType = type_name; + subscriberParam.topic.topicName = _create_topic_name(qos_policies, ros_topic_prefix, topic_name); + + if (!get_datareader_qos(*qos_policies, subscriberParam)) { + RMW_SET_ERROR_MSG("failed to get datareader qos"); + goto fail; + } + + info->listener_ = new (std::nothrow) SubListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); + goto fail; + } + + info->subscriber_ = Domain::createSubscriber(participant, subscriberParam, info->listener_); + if (!info->subscriber_) { + RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber"); + goto fail; + } + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->subscriber_->getGuid()); + rmw_subscription = rmw_subscription_allocate(); + if (!rmw_subscription) { + RMW_SET_ERROR_MSG("failed to allocate subscription"); + goto fail; + } + rmw_subscription->implementation_identifier = eprosima_fastrtps_identifier; + rmw_subscription->data = info; + rmw_subscription->topic_name = + reinterpret_cast(rmw_allocate(strlen(topic_name) + 1)); + + if (!rmw_subscription->topic_name) { + RMW_SET_ERROR_MSG("failed to allocate memory for subscription topic name"); + goto fail; + } + + memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); + + rmw_subscription->options = *subscription_options; + rmw_subscription->can_loan_messages = false; + return rmw_subscription; + +fail: + if (info != nullptr) { + delete info->type_support_; + delete info->listener_; + delete info; + } + type_registry.return_message_type_support(type_support); + rmw_subscription_free(rmw_subscription); + return nullptr; +} +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/rmw_fastrtps_shared_cpp/CMakeLists.txt b/rmw_fastrtps_shared_cpp/CMakeLists.txt index 32abbcb64..c3f98a0f9 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,13 @@ 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/init_rmw_context_impl.cpp + src/listener_thread.cpp src/namespace_prefix.cpp + src/participant.cpp + src/publisher.cpp src/qos.cpp src/rmw_client.cpp src/rmw_compare_gids_equal.cpp @@ -59,6 +65,7 @@ add_library(rmw_fastrtps_shared_cpp src/rmw_get_gid_for_publisher.cpp src/rmw_get_topic_endpoint_info.cpp src/rmw_guard_condition.cpp + src/rmw_init.cpp src/rmw_logging.cpp src/rmw_node.cpp src/rmw_node_info_and_types.cpp @@ -76,6 +83,7 @@ add_library(rmw_fastrtps_shared_cpp src/rmw_trigger_guard_condition.cpp src/rmw_wait.cpp src/rmw_wait_set.cpp + src/subscription.cpp src/TypeSupport_impl.cpp ) @@ -88,6 +96,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..44c8a31ff 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -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" @@ -28,11 +29,16 @@ #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; @@ -40,7 +46,6 @@ typedef struct CustomParticipantInfo { eprosima::fastrtps::Participant * participant; ::ParticipantListener * listener; - rmw_guard_condition_t * graph_guard_condition; // Flag to establish if the QoS of the participant, // its publishers and its subscribers are going @@ -53,88 +58,45 @@ 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( + const char * identifier, + rmw_dds_common::Context * context) + : context(context), + identifier_(identifier) {} void onParticipantDiscovery( eprosima::fastrtps::Participant *, eprosima::fastrtps::rtps::ParticipantDiscoveryInfo && info) override { - 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); + switch (info.status) { + case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DISCOVERED_PARTICIPANT: + { + auto map = rmw::impl::cpp::parse_key_value(info.info.m_userData); + auto name_found = map.find("securitycontext"); + + if (name_found == map.end()) { + return; + } + auto security_context = + std::string(name_found->second.begin(), name_found->second.end()); + + context->graph_cache.add_participant( + rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier_, info.info.m_guid), + security_context); + break; } - } - } - } - - 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; + case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT: + // fall through + case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT: + context->graph_cache.remove_participant( + rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier_, info.info.m_guid)); + break; + default: + return; } - return namespaces; } void onSubscriberDiscovery( @@ -159,43 +121,39 @@ 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); + + context->graph_cache.add_entity( + rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier_, + proxyData.guid()), proxyData.topicName().to_string(), proxyData.typeName().to_string(), - proxyData.m_qos); + rmw_fastrtps_shared_cpp::create_rmw_gid( + 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()); + context->graph_cache.remove_entity( + rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier_, + proxyData.guid()), + is_reader); } } - if (trigger) { - rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( - graph_guard_condition_->implementation_identifier, - graph_guard_condition_); - } } - 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_guard_condition_t * graph_guard_condition_; + rmw_dds_common::Context * context; + const char * const identifier_; }; #endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ 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/init_rmw_context_impl.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp new file mode 100644 index 000000000..13ca78dd8 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp @@ -0,0 +1,36 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ + +#include "rmw/init.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +/// Increment `rmw_context_impl_t` reference count, destroying it if the count reaches zero. +/** + * Function that should be called when destroying a node. + */ +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +decrement_context_impl_ref_count(rmw_context_t * context); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/listener_thread.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/listener_thread.hpp new file mode 100644 index 000000000..a4537c4f3 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/listener_thread.hpp @@ -0,0 +1,34 @@ +// 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__LISTENER_THREAD_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__LISTENER_THREAD_HPP_ + +#include "rmw/init.h" + +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +run_listener_thread(rmw_context_t * context); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +join_listener_thread(rmw_context_t * context); + +} // namespace rmw_fastrtps_shared_cpp +#endif // RMW_FASTRTPS_SHARED_CPP__LISTENER_THREAD_HPP_ 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..f4aff1c16 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/names.hpp @@ -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, +_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 +eprosima::fastrtps::string_255 +_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..d44f2fb94 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/participant.hpp @@ -0,0 +1,44 @@ +// 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" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +CustomParticipantInfo * +create_participant( + const char * identifier, + size_t domain_id, + const rmw_security_options_t * security_options, + bool localhost_only, + const char * security_context, + rmw_dds_common::Context * common_context); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +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..fa40362cb --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/publisher.hpp @@ -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. + +#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" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +destroy_publisher( + const char * identifier, + CustomParticipantInfo * participant_info, + rmw_publisher_t * publisher); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__PUBLISHER_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp index fed276204..1857abba7 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -15,11 +16,15 @@ #ifndef RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ #define RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ +#include "fastrtps/attributes/PublisherAttributes.h" +#include "fastrtps/attributes/SubscriberAttributes.h" +#include "fastrtps/qos/QosPolicies.h" +#include "fastrtps/qos/ReaderQos.h" +#include "fastrtps/qos/WriterQos.h" + #include "rmw/rmw.h" -#include "./visibility_control.h" -#include "fastrtps/qos/WriterQos.h" -#include "fastrtps/qos/ReaderQos.h" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" namespace eprosima { diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp index 13da08486..118d7a075 100644 --- a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -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 @@ -126,6 +124,15 @@ __rmw_init_event( void * data, rmw_event_type_t event_type); +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_node_names_with_security_contexts( + const char * identifier, + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * security_contexts); + RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t __rmw_publish( @@ -152,7 +159,7 @@ RMW_FASTRTPS_SHARED_CPP_PUBLIC rmw_ret_t __rmw_destroy_publisher( const char * identifier, - rmw_node_t * node, + const rmw_node_t * node, rmw_publisher_t * publisher); RMW_FASTRTPS_SHARED_CPP_PUBLIC @@ -270,7 +277,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..9132363b7 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_context_impl.hpp @@ -0,0 +1,32 @@ +// 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 +{ + /// Pointer to `rmw_dds_common::Context`. + void * common; + /// Pointer to `rmw_fastrtps_shared_cpp::CustomParticipantInfo`. + void * participant_info; + /// Mutex used to protect initialization/destruction. + std::mutex mutex; + /// Reference count. + uint64_t count; +}; + +#endif // RMW_FASTRTPS_SHARED_CPP__RMW_CONTEXT_IMPL_HPP_ diff --git a/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_init.hpp b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_init.hpp new file mode 100644 index 000000000..ce878fa05 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/rmw_init.hpp @@ -0,0 +1,43 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__RMW_INIT_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__RMW_INIT_HPP_ + +#include "rmw/init.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/init_options.h" + +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +rmw_init_options_init( + const char * identifier, rmw_init_options_t * init_options, rcutils_allocator_t allocator); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +rmw_init_options_copy( + const char * identifier, const rmw_init_options_t * src, rmw_init_options_t * dst); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +rmw_init_options_fini(const char * identifier, rmw_init_options_t * init_options); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__RMW_INIT_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..523b916b3 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/include/rmw_fastrtps_shared_cpp/subscription.hpp @@ -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. + +#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" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +destroy_subscription( + const char * identifier, + CustomParticipantInfo * participant_info, + rmw_subscription_t * subscription); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__SUBSCRIPTION_HPP_ diff --git a/rmw_fastrtps_shared_cpp/package.xml b/rmw_fastrtps_shared_cpp/package.xml index e0a0b8931..837d1f117 100644 --- a/rmw_fastrtps_shared_cpp/package.xml +++ b/rmw_fastrtps_shared_cpp/package.xml @@ -19,6 +19,7 @@ rcpputils rcutils rmw + rmw_dds_common fastcdr fastrtps @@ -26,6 +27,7 @@ rcpputils rcutils rmw + rmw_dds_common ament_lint_auto ament_lint_common diff --git a/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp b/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp new file mode 100644 index 000000000..72ffb3087 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/create_rmw_gid.cpp @@ -0,0 +1,36 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "fastrtps/rtps/common/Guid.h" + +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp" +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" + +rmw_gid_t +rmw_fastrtps_shared_cpp::create_rmw_gid( + const char * identifier, const eprosima::fastrtps::rtps::GUID_t & guid) +{ + rmw_gid_t rmw_gid = {}; + rmw_gid.implementation_identifier = identifier; + static_assert( + sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, + "RMW_GID_STORAGE_SIZE insufficient to store the fastrtps GUID_t." + ); + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + guid, + rmw_gid.data); + return rmw_gid; +} diff --git a/rmw_fastrtps_shared_cpp/src/demangle.cpp b/rmw_fastrtps_shared_cpp/src/demangle.cpp index 752355888..15e4e3f2d 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,61 +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) { + } else { 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 "". @@ -154,3 +159,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/init_rmw_context_impl.cpp b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp new file mode 100644 index 000000000..58332a061 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/init_rmw_context_impl.cpp @@ -0,0 +1,107 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp" + +#include + +#include "rmw/error_handling.h" +#include "rmw/init.h" + +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/participant.hpp" +#include "rmw_fastrtps_shared_cpp/publisher.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +#include "rmw_fastrtps_shared_cpp/listener_thread.hpp" + +rmw_ret_t +rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(rmw_context_t * context) +{ + std::lock_guard guard(context->impl->mutex); + + assert(context); + assert(context->impl); + assert(0u < context->impl->count); + + if (0u == --context->impl->count) { + rmw_ret_t err = RMW_RET_OK; + rmw_ret_t ret = RMW_RET_OK; + rmw_error_string_t error_string; + + ret = rmw_fastrtps_shared_cpp::join_listener_thread(context); + if (RMW_RET_OK != ret) { + return ret; + } + + auto common_context = static_cast(context->impl->common); + auto participant_info = static_cast(context->impl->participant_info); + + if (!common_context->graph_cache.remove_participant(common_context->gid)) { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " + "couldn't remove Participant gid from graph_cache when destroying Participant"); + } + + ret = rmw_fastrtps_shared_cpp::destroy_subscription( + context->implementation_identifier, + participant_info, + common_context->sub); + // Try to clean the other objects if the above failed. + if (RMW_RET_OK != ret) { + error_string = rmw_get_error_string(); + rmw_reset_error(); + } + err = rmw_fastrtps_shared_cpp::destroy_publisher( + context->implementation_identifier, + participant_info, + common_context->pub); + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + // We can just return one error, log about the previous one. + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_subscription' failed\n"); + ret = err; + error_string = rmw_get_error_string(); + rmw_reset_error(); + } + err = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_publisher' failed\n"); + ret = err; + } else if (RMW_RET_OK != ret) { + RMW_SET_ERROR_MSG(error_string.str); + } + + common_context->graph_cache.clear_on_change_callback(); + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->graph_guard_condition)) + { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " + "couldn't destroy graph_guard_condtion"); + } + + delete common_context; + context->impl->common = nullptr; + context->impl->participant_info = nullptr; + } + return RMW_RET_OK; +} diff --git a/rmw_fastrtps_shared_cpp/src/listener_thread.cpp b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp new file mode 100644 index 000000000..8df2bb70b --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/listener_thread.cpp @@ -0,0 +1,177 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#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/listener_thread.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +using rmw_dds_common::operator<<; + +static +void +node_listener(rmw_context_t * context); + +rmw_ret_t +rmw_fastrtps_shared_cpp::run_listener_thread(rmw_context_t * context) +{ + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.store(true); + common_context->listener_thread_gc = rmw_fastrtps_shared_cpp::__rmw_create_guard_condition( + context->implementation_identifier); + if (common_context->listener_thread_gc) { + try { + common_context->listener_thread = std::thread(node_listener, context); + 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_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->listener_thread_gc)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" + RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); + } + } + return RMW_RET_ERROR; +} + +rmw_ret_t +rmw_fastrtps_shared_cpp::join_listener_thread(rmw_context_t * context) +{ + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.exchange(false); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + context->implementation_identifier, 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_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->listener_thread_gc); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + return RMW_RET_OK; +} + +#define TERMINATE_THREAD(msg) \ + { \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ + RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) \ + ": ros discovery info listener thread will shutdown ...\n"); \ + break; \ + } + +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_fastrtps_shared_cpp::__rmw_create_wait_set( + context->implementation_identifier, context, 2); + if (nullptr == wait_set) { + TERMINATE_THREAD("failed to create wait set"); + } + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_wait( + &subscriptions, + &guard_conditions, + nullptr, + nullptr, + nullptr, + wait_set, + nullptr)) + { + TERMINATE_THREAD("rmw_wait failed"); + } + if (subscriptions_buffer[0]) { + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + bool taken; + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_take( + context->implementation_identifier, + common_context->sub, + static_cast(&msg), + &taken, + nullptr)) + { + TERMINATE_THREAD("__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_fastrtps_shared_cpp::__rmw_destroy_wait_set( + context->implementation_identifier, wait_set)) + { + TERMINATE_THREAD("failed to destroy wait set"); + } + } +} 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..2f6cad6ae --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/participant.cpp @@ -0,0 +1,265 @@ +// 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/filesystem.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; + +#if HAVE_SECURITY +static +bool +get_security_file_paths( + std::array & security_files_paths, const char * 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(secure_root, file_names[i], allocator); + + if (!file_path) { + return false; + } + + if (rcutils_is_readable(file_path)) { + security_files_paths[i] = file_prefix + std::string(file_path); + } else { + allocator.deallocate(file_path, allocator.state); + return false; + } + + allocator.deallocate(file_path, allocator.state); + } + + return true; +} +#endif + +static +CustomParticipantInfo * +__create_participant( + 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; + CustomParticipantInfo * participant_info = nullptr; + + try { + listener = new ::ParticipantListener( + identifier, 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; + + return participant_info; +fail: + rmw_free(listener); + if (participant) { + Domain::removeParticipant(participant); + } + return nullptr; +} + +CustomParticipantInfo * +rmw_fastrtps_shared_cpp::create_participant( + const char * identifier, + size_t domain_id, + const rmw_security_options_t * security_options, + bool localhost_only, + const char * security_context, + 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); + + size_t length = snprintf(nullptr, 0, "securitycontext=%s;", security_context) + 1; + participantAttrs.rtps.userData.resize(length); + int written = snprintf( + reinterpret_cast(participantAttrs.rtps.userData.data()), + length, "securitycontext=%s;", security_context); + if (written < 0 || written > static_cast(length) - 1) { + RMW_SET_ERROR_MSG("failed to populate user_data buffer"); + return nullptr; + } + participantAttrs.rtps.setName(security_context); + + 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); + 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/rmw_client.cpp b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp index b9f77310b..ec5237216 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_client.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_client.cpp @@ -24,6 +24,7 @@ #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/qos.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" #include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" using Domain = eprosima::fastrtps::Domain; @@ -48,7 +49,33 @@ __rmw_destroy_client( return RMW_RET_ERROR; } + auto common_context = static_cast(node->context->impl->common); auto info = static_cast(client->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->request_publisher_->getGuid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->response_subscriber_->getGuid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( + gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + } + if (info != nullptr) { if (info->response_subscriber_ != nullptr) { Domain::removeSubscriber(info->response_subscriber_); diff --git a/rmw_fastrtps_shared_cpp/src/rmw_count.cpp b/rmw_fastrtps_shared_cpp/src/rmw_count.cpp index 0da58e436..a30d21989 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_count.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_count.cpp @@ -24,10 +24,12 @@ #include "rmw/rmw.h" #include "rmw/types.h" -#include "demangle.hpp" +#include "rmw_dds_common/context.hpp" + #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" namespace rmw_fastrtps_shared_cpp { @@ -39,53 +41,18 @@ __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).to_string(); + return common_context->graph_cache.get_writer_count(mangled_topic_name, count); } rmw_ret_t @@ -95,52 +62,17 @@ __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).to_string(); + return common_context->graph_cache.get_reader_count(mangled_topic_name, count); } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp index 7ca05d892..c6cd2bbda 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_get_topic_endpoint_info.cpp @@ -24,9 +24,11 @@ #include "rmw/topic_endpoint_info.h" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" -#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +#include "rmw_dds_common/graph_cache.hpp" #include "demangle.hpp" @@ -75,246 +77,37 @@ _validate_params( return RMW_RET_OK; } -std::vector -_get_topic_fqdns(const char * topic_name, bool no_mangle) -{ - std::vector topic_fqdns; - topic_fqdns.push_back(topic_name); - if (!no_mangle) { - auto ros_prefixes = _get_all_ros_prefixes(); - // Build the list of all possible topic FQDN - std::for_each( - ros_prefixes.begin(), ros_prefixes.end(), - [&topic_fqdns, &topic_name](const std::string & prefix) { - topic_fqdns.push_back(prefix + topic_name); - }); - } - return topic_fqdns; -} - -void -_handle_topic_endpoint_info_fini( - rmw_topic_endpoint_info_t * topic_endpoint_info, - rcutils_allocator_t * allocator) -{ - bool had_error = rmw_error_is_set(); - std::string error_string; - if (had_error) { - error_string = rmw_get_error_string().str; - } - rmw_reset_error(); - - rmw_ret_t ret = rmw_topic_endpoint_info_fini(topic_endpoint_info, allocator); - if (ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_cpp", - "rmw_topic_endpoint_info_fini failed: %s", - rmw_get_error_string().str); - rmw_reset_error(); - } - - if (had_error) { - RMW_SET_ERROR_MSG(error_string.c_str()); - } -} - -rmw_ret_t -_set_rmw_topic_endpoint_info( - rmw_topic_endpoint_info_t * topic_endpoint_info, - const GUID_t & participant_guid, - const char * node_name, - const char * node_namespace, - TopicData topic_data, - bool no_mangle, - bool is_publisher, - ::ParticipantListener * slave_target, - rcutils_allocator_t * allocator) -{ - static_assert( - sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, - "RMW_GID_STORAGE_SIZE insufficient to store the rmw_fastrtps_cpp GID implementation." - ); - rmw_ret_t ret; - // set endpoint type - if (is_publisher) { - ret = rmw_topic_endpoint_info_set_endpoint_type(topic_endpoint_info, RMW_ENDPOINT_PUBLISHER); - } else { - ret = rmw_topic_endpoint_info_set_endpoint_type(topic_endpoint_info, RMW_ENDPOINT_SUBSCRIPTION); - } - if (ret != RMW_RET_OK) { - return ret; - } - // set endpoint gid - uint8_t rmw_gid[RMW_GID_STORAGE_SIZE] = {}; - rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( - topic_data.entity_guid, - rmw_gid); - ret = rmw_topic_endpoint_info_set_gid( - topic_endpoint_info, - rmw_gid, - sizeof(eprosima::fastrtps::rtps::GUID_t)); - if (ret != RMW_RET_OK) { - return ret; - } - // set qos profile - ret = rmw_topic_endpoint_info_set_qos_profile(topic_endpoint_info, &topic_data.qos_profile); - if (ret != RMW_RET_OK) { - return ret; - } - // set topic type - std::string type_name = - no_mangle ? topic_data.topic_type : _demangle_if_ros_type(topic_data.topic_type); - ret = rmw_topic_endpoint_info_set_topic_type(topic_endpoint_info, type_name.c_str(), allocator); - if (ret != RMW_RET_OK) { - return ret; - } - // Check if this participant is the same as the node that is passed - if (topic_data.participant_guid == participant_guid) { - ret = rmw_topic_endpoint_info_set_node_name(topic_endpoint_info, node_name, allocator); - if (ret != RMW_RET_OK) { - return ret; - } - ret = - rmw_topic_endpoint_info_set_node_namespace(topic_endpoint_info, node_namespace, allocator); - if (ret != RMW_RET_OK) { - return ret; - } - return ret; - } - // This means that this discovered participant is not associated with the passed node - // and hence we must find its name and namespace from the discovered_names and - // discovered_namespace maps - // set node name - { // Scope for lock guard - std::lock_guard guard(slave_target->names_mutex_); - const auto & d_name_it = slave_target->discovered_names.find(topic_data.participant_guid); - if (d_name_it != slave_target->discovered_names.end()) { - ret = rmw_topic_endpoint_info_set_node_name( - topic_endpoint_info, d_name_it->second.c_str(), allocator); - } else { - ret = rmw_topic_endpoint_info_set_node_name( - topic_endpoint_info, "_NODE_NAME_UNKNOWN_", allocator); - } - if (ret != RMW_RET_OK) { - return ret; - } - // set node namespace - const auto & d_namespace_it = - slave_target->discovered_namespaces.find(topic_data.participant_guid); - if (d_namespace_it != slave_target->discovered_namespaces.end()) { - ret = rmw_topic_endpoint_info_set_node_namespace( - topic_endpoint_info, d_namespace_it->second.c_str(), allocator); - } else { - ret = rmw_topic_endpoint_info_set_node_namespace( - topic_endpoint_info, "_NODE_NAMESPACE_UNKNOWN_", allocator); - } - } - return ret; -} - rmw_ret_t -_get_info_by_topic( +__rmw_get_publishers_info_by_topic( const char * identifier, const rmw_node_t * node, rcutils_allocator_t * allocator, const char * topic_name, bool no_mangle, - bool is_publisher, - rmw_topic_endpoint_info_array_t * participants_info) + rmw_topic_endpoint_info_array_t * publishers_info) { rmw_ret_t ret = _validate_params( identifier, node, allocator, topic_name, - participants_info); + publishers_info); if (ret != RMW_RET_OK) { return ret; } - - const auto & topic_fqdns = _get_topic_fqdns(topic_name, no_mangle); - auto impl = static_cast(node->data); - // The GUID of the participant associated with this node - const auto & participant_guid = impl->participant->getGuid(); - const auto & node_name = node->name; - const auto & node_namespace = node->namespace_; - ::ParticipantListener * slave_target = impl->listener; - auto & topic_cache = - is_publisher ? slave_target->writer_topic_cache : slave_target->reader_topic_cache; - { - std::lock_guard guard(topic_cache.getMutex()); - const auto & topic_name_to_data = topic_cache().getTopicNameToTopicData(); - std::vector topic_endpoint_info_vector; - for (const auto & topic_name : topic_fqdns) { - const auto it = topic_name_to_data.find(topic_name); - if (it != topic_name_to_data.end()) { - for (const auto & data : it->second) { - rmw_topic_endpoint_info_t topic_endpoint_info = - rmw_get_zero_initialized_topic_endpoint_info(); - rmw_ret_t ret = _set_rmw_topic_endpoint_info( - &topic_endpoint_info, - participant_guid, - node_name, - node_namespace, - data, - no_mangle, - is_publisher, - slave_target, - allocator); - if (ret != RMW_RET_OK) { - // Free topic_endpoint_info - _handle_topic_endpoint_info_fini(&topic_endpoint_info, allocator); - // Free all the space allocated to the previous topic_endpoint_infos - for (auto & tinfo : topic_endpoint_info_vector) { - _handle_topic_endpoint_info_fini(&tinfo, allocator); - } - return ret; - } - // add rmw_topic_endpoint_info_t to a vector - topic_endpoint_info_vector.push_back(topic_endpoint_info); - } - } - } - - // add all the elements from the vector to rmw_topic_endpoint_info_array_t - auto count = topic_endpoint_info_vector.size(); - ret = rmw_topic_endpoint_info_array_init_with_size(participants_info, count, allocator); - if (ret != RMW_RET_OK) { - rmw_error_string_t error_message = rmw_get_error_string(); - rmw_reset_error(); - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "rmw_topic_endpoint_info_array_init_with_size failed to allocate memory: %s", - error_message.str); - // Free all the space allocated to the previous topic_endpoint_infos - for (auto & tinfo : topic_endpoint_info_vector) { - _handle_topic_endpoint_info_fini(&tinfo, allocator); - } - return ret; - } - for (auto i = 0u; i < count; i++) { - participants_info->info_array[i] = topic_endpoint_info_vector.at(i); - } + auto common_context = static_cast(node->context->impl->common); + 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).to_string(); + 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, + return common_context->graph_cache.get_writers_info_by_topic( + mangled_topic_name, + demangle_type, allocator, - topic_name, - no_mangle, - true, /*is_publisher*/ publishers_info); } @@ -327,13 +120,27 @@ __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); + 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).to_string(); + demangle_type = _demangle_if_ros_type; + } + + return common_context->graph_cache.get_readers_info_by_topic( + mangled_topic_name, + demangle_type, + allocator, subscriptions_info); } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_init.cpp b/rmw_fastrtps_shared_cpp/src/rmw_init.cpp new file mode 100644 index 000000000..9bd8c5666 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/rmw_init.cpp @@ -0,0 +1,100 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw_fastrtps_shared_cpp/rmw_init.hpp" + +#include "rcutils/strdup.h" +#include "rcutils/types.h" + +#include "rmw/init.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/init_options.h" + +namespace rmw_fastrtps_shared_cpp +{ + +rmw_ret_t +rmw_init_options_init( + const char * identifier, rmw_init_options_t * init_options, rcutils_allocator_t allocator) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); + if (NULL != init_options->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized init_options"); + return RMW_RET_INVALID_ARGUMENT; + } + init_options->instance_id = 0; + init_options->implementation_identifier = identifier; + init_options->allocator = allocator; + init_options->impl = nullptr; + init_options->security_context = NULL; + init_options->domain_id = RMW_DEFAULT_DOMAIN_ID; + init_options->security_options = rmw_get_default_security_options(); + init_options->localhost_only = RMW_LOCALHOST_ONLY_DEFAULT; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_init_options_copy( + const char * identifier, const rmw_init_options_t * src, rmw_init_options_t * dst) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + src, + src->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (NULL != dst->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized dst"); + return RMW_RET_INVALID_ARGUMENT; + } + const rcutils_allocator_t * allocator = &src->allocator; + rmw_ret_t ret = RMW_RET_OK; + + allocator->deallocate(dst->security_context, allocator->state); + *dst = *src; + dst->security_context = NULL; + dst->security_options = rmw_get_zero_initialized_security_options(); + + dst->security_context = rcutils_strdup(src->security_context, *allocator); + if (src->security_context && !dst->security_context) { + ret = RMW_RET_BAD_ALLOC; + goto fail; + } + return rmw_security_options_copy(&src->security_options, allocator, &dst->security_options); +fail: + allocator->deallocate(dst->security_context, allocator->state); + return ret; +} + +rmw_ret_t +rmw_init_options_fini(const char * identifier, rmw_init_options_t * init_options) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + rcutils_allocator_t & allocator = init_options->allocator; + RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + init_options, + init_options->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + allocator.deallocate(init_options->security_context, allocator.state); + rmw_security_options_fini(&init_options->security_options, &allocator); + *init_options = rmw_get_zero_initialized_init_options(); + return RMW_RET_OK; +} + +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node.cpp index be653928a..d59789289 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,11 +14,11 @@ // limitations under the License. #include +#include #include #include #include -#include "rcutils/filesystem.h" #include "rcutils/logging_macros.h" #include "rmw/allocators.h" @@ -25,45 +26,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 +52,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 +62,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 +80,26 @@ create_node( goto fail; } memcpy(const_cast(node_handle->namespace_), namespace_, strlen(namespace_) + 1); - + node_handle->context = context; + + { + // Though graph_cache methods are thread safe, both cache update and publishing have to also + // be atomic. + // If not, the following race condition is possible: + // node1-update-get-message / node2-update-get-message / node2-publish / node1-publish + // In that case, the last message published is not accurate. + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo participant_msg = + graph_cache.add_node(common_context->gid, name, namespace_); + if (RMW_RET_OK != __rmw_publish( + identifier, + common_context->pub, + static_cast(&participant_msg), + nullptr)) + { + goto fail; + } + } return node_handle; fail: if (node_handle) { @@ -164,184 +109,9 @@ create_node( node_handle->name = nullptr; } rmw_node_free(node_handle); - delete node_impl; - if (graph_guard_condition) { - rmw_ret_t ret = __rmw_destroy_guard_condition(graph_guard_condition); - if (ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_shared_cpp", - "failed to destroy guard condition during error handling"); - } - } - rmw_free(listener); - if (participant) { - Domain::removeParticipant(participant); - } return nullptr; } -bool -get_security_file_paths( - std::array & security_files_paths, const char * node_secure_root) -{ - // here assume only 6 files for security - const char * file_names[6] = { - "identity_ca.cert.pem", "cert.pem", "key.pem", - "permissions_ca.cert.pem", "governance.p7s", "permissions.p7s" - }; - size_t num_files = sizeof(file_names) / sizeof(char *); - - std::string file_prefix("file://"); - - for (size_t i = 0; i < num_files; i++) { - rcutils_allocator_t allocator = rcutils_get_default_allocator(); - char * file_path = rcutils_join_path(node_secure_root, file_names[i], allocator); - - if (!file_path) { - return false; - } - - if (rcutils_is_readable(file_path)) { - security_files_paths[i] = file_prefix + std::string(file_path); - } else { - allocator.deallocate(file_path, allocator.state); - return false; - } - - allocator.deallocate(file_path, allocator.state); - } - - return true; -} - -rmw_node_t * -__rmw_create_node( - const char * identifier, - const char * name, - const char * namespace_, - size_t domain_id, - const rmw_node_security_options_t * security_options, - bool localhost_only) -{ - if (!name) { - RMW_SET_ERROR_MSG("name is null"); - return nullptr; - } - if (!security_options) { - RMW_SET_ERROR_MSG("security_options is null"); - return nullptr; - } - - ParticipantAttributes participantAttrs; - - // Load default XML profile. - Domain::getDefaultParticipantAttributes(participantAttrs); - - participantAttrs.rtps.builtin.domainId = static_cast(domain_id); - // since the participant name is not part of the DDS spec - participantAttrs.rtps.setName(name); - - if (localhost_only) { - Locator_t local_network_interface_locator; - static const std::string local_ip_name("127.0.0.1"); - local_network_interface_locator.kind = 1; - local_network_interface_locator.port = 0; - IPLocator::setIPv4(local_network_interface_locator, local_ip_name); - participantAttrs.rtps.builtin.metatrafficUnicastLocatorList.push_back( - local_network_interface_locator); - participantAttrs.rtps.builtin.initialPeersList.push_back(local_network_interface_locator); - } - bool leave_middleware_default_qos = false; - const char * env_var = "RMW_FASTRTPS_USE_QOS_FROM_XML"; - // Check if the configuration from XML has been enabled from - // the RMW_FASTRTPS_USE_QOS_FROM_XML env variable. - char * config_env_val = nullptr; -#ifndef _WIN32 - config_env_val = getenv(env_var); - if (config_env_val != nullptr) { - leave_middleware_default_qos = strcmp(config_env_val, "1") == 0; - } -#else - size_t config_env_val_size; - _dupenv_s(&config_env_val, &config_env_val_size, env_var); - if (config_env_val != nullptr) { - leave_middleware_default_qos = strcmp(config_env_val, "1") == 0; - } - free(config_env_val); -#endif - - // allow reallocation to support discovery messages bigger than 5000 bytes - if (!leave_middleware_default_qos) { - participantAttrs.rtps.builtin.readerHistoryMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - participantAttrs.rtps.builtin.writerHistoryMemoryPolicy = - eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; - } - - size_t length = strlen(name) + strlen("name=;") + - strlen(namespace_) + strlen("namespace=;") + 1; - participantAttrs.rtps.userData.resize(length); - int written = snprintf( - reinterpret_cast(participantAttrs.rtps.userData.data()), - length, "name=%s;namespace=%s;", name, namespace_); - if (written < 0 || written > static_cast(length) - 1) { - RMW_SET_ERROR_MSG("failed to populate user_data buffer"); - return nullptr; - } - - if (security_options->security_root_path) { - // if security_root_path provided, try to find the key and certificate files -#if HAVE_SECURITY - std::array security_files_paths; - - if (get_security_file_paths(security_files_paths, security_options->security_root_path)) { - eprosima::fastrtps::rtps::PropertyPolicy property_policy; - using Property = eprosima::fastrtps::rtps::Property; - property_policy.properties().emplace_back( - Property("dds.sec.auth.plugin", "builtin.PKI-DH")); - property_policy.properties().emplace_back( - Property( - "dds.sec.auth.builtin.PKI-DH.identity_ca", - security_files_paths[0])); - property_policy.properties().emplace_back( - Property( - "dds.sec.auth.builtin.PKI-DH.identity_certificate", - security_files_paths[1])); - property_policy.properties().emplace_back( - Property( - "dds.sec.auth.builtin.PKI-DH.private_key", - security_files_paths[2])); - property_policy.properties().emplace_back( - Property("dds.sec.crypto.plugin", "builtin.AES-GCM-GMAC")); - - property_policy.properties().emplace_back( - Property( - "dds.sec.access.plugin", "builtin.Access-Permissions")); - property_policy.properties().emplace_back( - Property( - "dds.sec.access.builtin.Access-Permissions.permissions_ca", security_files_paths[3])); - property_policy.properties().emplace_back( - Property( - "dds.sec.access.builtin.Access-Permissions.governance", security_files_paths[4])); - property_policy.properties().emplace_back( - Property( - "dds.sec.access.builtin.Access-Permissions.permissions", security_files_paths[5])); - - participantAttrs.rtps.properties = property_policy; - } else if (security_options->enforce_security) { - RMW_SET_ERROR_MSG("couldn't find all security files!"); - return nullptr; - } -#else - RMW_SET_ERROR_MSG( - "This Fast-RTPS version doesn't have the security libraries\n" - "Please compile Fast-RTPS using the -DSECURITY=ON CMake option"); - return nullptr; -#endif - } - return create_node(identifier, name, namespace_, participantAttrs); -} - rmw_ret_t __rmw_destroy_node( const char * identifier, @@ -358,33 +128,28 @@ __rmw_destroy_node( return RMW_RET_ERROR; } - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return RMW_RET_ERROR; + auto common_context = static_cast(node->context->impl->common); + rmw_dds_common::GraphCache & graph_cache = common_context->graph_cache; + { + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo participant_msg = + graph_cache.remove_node(common_context->gid, node->name, node->namespace_); + result_ret = __rmw_publish( + identifier, + common_context->pub, + static_cast(&participant_msg), + nullptr); + if (RMW_RET_OK != result_ret) { + return result_ret; + } } - - Participant * participant = impl->participant; - - // Begin deleting things in the same order they were created in __rmw_create_node(). rmw_free(const_cast(node->name)); node->name = nullptr; rmw_free(const_cast(node->namespace_)); node->namespace_ = nullptr; rmw_node_free(node); - Domain::removeParticipant(participant); - - if (RMW_RET_OK != __rmw_destroy_guard_condition(impl->graph_guard_condition)) { - RMW_SET_ERROR_MSG("failed to destroy graph guard condition"); - result_ret = RMW_RET_ERROR; - } - - delete impl->listener; - impl->listener = nullptr; - delete impl; - - return result_ret; + return RMW_RET_OK; } rmw_ret_t @@ -392,35 +157,20 @@ __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); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); + auto common_context = static_cast(node->context->impl->common); + if (!common_context) { + RMW_SET_ERROR_MSG("common_context is nullptr"); return nullptr; } - return impl->graph_guard_condition; + return common_context->graph_guard_condition; } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp b/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp index 58e15f4d6..4d95448dd 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_node_info_and_types.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2018 Amazon.com, Inc. or its affiliates. All Rights Reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -32,68 +33,18 @@ #include "rmw/names_and_types.h" #include "rmw/rmw.h" +#include "rmw_dds_common/context.hpp" + #include "demangle.hpp" #include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" #include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" - -#include "rmw_fastrtps_shared_cpp/topic_cache.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" namespace rmw_fastrtps_shared_cpp { -constexpr char kLoggerTag[] = "rmw_fastrtps_shared_cpp"; - -/** - * Get the guid that corresponds to the node and namespace. - * - * \param node to discover other participants with - * \param node_name of the desired node - * \param node_namespace of the desired node - * \param guid [out] result - * \return RMW_RET_ERROR if unable to find guid - * \return RMW_RET_OK if guid is available - */ -rmw_ret_t __get_guid_by_name( - const rmw_node_t * node, const char * node_name, - const char * node_namespace, GUID_t & guid) -{ - auto impl = static_cast(node->data); - if (strcmp(node->name, node_name) == 0 && strcmp(node->namespace_, node_namespace) == 0) { - guid = impl->participant->getGuid(); - } else { - std::set nodes_in_desired_namespace; - std::lock_guard guard(impl->listener->names_mutex_); - - auto namespaces = impl->listener->discovered_namespaces; - for (auto & guid_to_namespace : impl->listener->discovered_namespaces) { - if (guid_to_namespace.second == node_namespace) { - nodes_in_desired_namespace.insert(guid_to_namespace.first); - } - } - - auto guid_node_pair = std::find_if( - impl->listener->discovered_names.begin(), - impl->listener->discovered_names.end(), - [node_name, &nodes_in_desired_namespace](const std::pair & pair) { - return pair.second == node_name && - nodes_in_desired_namespace.find(pair.first) != nodes_in_desired_namespace.end(); - }); - - if (guid_node_pair == impl->listener->discovered_names.end()) { - RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( - "Node name not found: ns='%s', name='%s'", - node_namespace, - node_name - ); - return RMW_RET_NODE_NAME_NON_EXISTENT; - } - guid = guid_node_pair->first; - } - return RMW_RET_OK; -} - /** * Validate the input data of node_info_and_types functions. * @@ -141,189 +92,15 @@ rmw_ret_t __validate_input( return RMW_RET_OK; } -/** - * Access the slave Listeners, which are the ones that have the topicnamesandtypes member - * Get info from publisher and subscriber - * Combined results from the two lists - * - * \param topic_cache cache with topic information - * \param topics [out] resulting topics - * \param node_guid_ to find information for - * \param no_demangle true if demangling will not occur - */ -void -__accumulate_topics( - const LockedObject & topic_cache, - std::map> & topics, - const GUID_t & node_guid_, - bool no_demangle) -{ - std::lock_guard guard(topic_cache.getMutex()); - const auto & node_topics = topic_cache().getParticipantToTopics().find(node_guid_); - if (node_topics == topic_cache().getParticipantToTopics().end()) { - RCUTILS_LOG_DEBUG_NAMED( - kLoggerTag, - "No topics found for node"); - return; - } - for (auto & topic_pair : node_topics->second) { - if (!no_demangle && _get_ros_prefix_if_exists(topic_pair.first) != ros_topic_prefix) { - // if we are demangling and this is not prefixed with rt/, skip it - continue; - } - RCUTILS_LOG_DEBUG_NAMED( - kLoggerTag, - "accumulate_topics: Found topic %s", - topic_pair.first.c_str()); - - topics[topic_pair.first].insert( - topic_pair.second.begin(), topic_pair.second.end()); - } -} +using GetNamesAndTypesByNodeFunction = rmw_ret_t (*)( + rmw_dds_common::Context *, + const std::string &, + const std::string &, + DemangleFunction, + DemangleFunction, + rcutils_allocator_t *, + rmw_names_and_types_t *); -/** - * Copy topic data to results - * - * \param topics to copy over - * \param allocator to use - * \param no_demangle true if demangling will not occur - * \param topic_names_and_types [out] final rmw result - * \return RMW_RET_OK if successful - */ -rmw_ret_t -__copy_data_to_results( - const std::map> & topics, - rcutils_allocator_t * allocator, - bool no_demangle, - rmw_names_and_types_t * topic_names_and_types) -{ - // Copy data to results handle - if (!topics.empty()) { - // Setup string array to store names - rmw_ret_t rmw_ret = rmw_names_and_types_init(topic_names_and_types, topics.size(), allocator); - if (rmw_ret != RMW_RET_OK) { - return rmw_ret; - } - // Setup cleanup function, in case of failure below - auto fail_cleanup = [&topic_names_and_types]() { - rmw_ret_t rmw_ret = rmw_names_and_types_fini(topic_names_and_types); - if (rmw_ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - kLoggerTag, - "error during report of error: %s", rmw_get_error_string().str); - } - }; - // Setup demangling functions based on no_demangle option - auto demangle_topic = _demangle_if_ros_topic; - auto demangle_type = _demangle_if_ros_type; - if (no_demangle) { - auto noop = [](const std::string & in) { - return in; - }; - demangle_topic = noop; - demangle_type = noop; - } - // For each topic, store the name, initialize the string array for types, and store all types - size_t index = 0; - for (const auto & topic_n_types : topics) { - // Duplicate and store the topic_name - char * topic_name = rcutils_strdup(demangle_topic(topic_n_types.first).c_str(), *allocator); - if (!topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for topic name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - topic_names_and_types->names.data[index] = topic_name; - // Setup storage for types - { - rcutils_ret_t rcutils_ret = rcutils_string_array_init( - &topic_names_and_types->types[index], - topic_n_types.second.size(), - allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RMW_SET_ERROR_MSG(rcutils_get_error_string().str); - fail_cleanup(); - return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); - } - } - // Duplicate and store each type for the topic - size_t type_index = 0; - for (const auto & type : topic_n_types.second) { - char * type_name = rcutils_strdup(demangle_type(type).c_str(), *allocator); - if (!type_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for type name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - topic_names_and_types->types[index].data[type_index] = type_name; - ++type_index; - } // for each type - ++index; - } // for each topic - } - return RMW_RET_OK; -} - -void -__log_debug_information(const CustomParticipantInfo & impl) -{ - if (rcutils_logging_logger_is_enabled_for(kLoggerTag, RCUTILS_LOG_SEVERITY_DEBUG)) { - { - auto & topic_cache = impl.listener->writer_topic_cache; - std::lock_guard guard(topic_cache.getMutex()); - std::stringstream map_ss; - map_ss << topic_cache(); - RCUTILS_LOG_DEBUG_NAMED( - kLoggerTag, - "Publisher Topic cache is: %s", map_ss.str().c_str()); - } - { - auto & topic_cache = impl.listener->reader_topic_cache; - std::lock_guard guard(topic_cache.getMutex()); - std::stringstream map_ss; - map_ss << topic_cache(); - RCUTILS_LOG_DEBUG_NAMED( - kLoggerTag, - "Subscriber Topic cache is: %s", map_ss.str().c_str()); - } - { - std::stringstream ss; - std::lock_guard guard(impl.listener->names_mutex_); - for (auto & node_pair : impl.listener->discovered_names) { - ss << node_pair.first << " : " << node_pair.second << " "; - } - RCUTILS_LOG_DEBUG_NAMED(kLoggerTag, "Discovered names: %s", ss.str().c_str()); - } - { - std::stringstream ss; - std::lock_guard guard(impl.listener->names_mutex_); - for (auto & node_pair : impl.listener->discovered_namespaces) { - ss << node_pair.first << " : " << node_pair.second << " "; - } - RCUTILS_LOG_DEBUG_NAMED(kLoggerTag, "Discovered namespaces: %s", ss.str().c_str()); - } - } -} - -/** - * Function to abstract which topic_cache to use when gathering information. - */ -typedef std::function&(CustomParticipantInfo & participant_info)> - RetrieveCache; - -/** - * Get topic names and types for the specific node_name and node_namespace requested. - * - * \param identifier corresponding to the input node - * \param node to use for discovery - * \param allocator for returned value - * \param node_name to search - * \param node_namespace to search - * \param no_demangle true if the topics should not be demangled - * \param retrieve_cache_func getter for topic cache - * \param topic_names_and_types result - * \return RMW_RET_OK if successful - */ rmw_ret_t __rmw_get_topic_names_and_types_by_node( const char * identifier, @@ -331,8 +108,10 @@ __rmw_get_topic_names_and_types_by_node( rcutils_allocator_t * allocator, const char * node_name, const char * node_namespace, + DemangleFunction demangle_topic, + DemangleFunction demangle_type, bool no_demangle, - RetrieveCache & retrieve_cache_func, + GetNamesAndTypesByNodeFunction get_names_and_types_by_node, rmw_names_and_types_t * topic_names_and_types) { rmw_ret_t valid_input = __validate_input( @@ -340,42 +119,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, @@ -384,125 +184,40 @@ __rmw_get_publisher_names_and_types_by_node( bool no_demangle, rmw_names_and_types_t * topic_names_and_types) { - RetrieveCache retrieve_pub_cache = - [](CustomParticipantInfo & participant_info) -> const LockedObject & { - return participant_info.listener->writer_topic_cache; - }; return __rmw_get_topic_names_and_types_by_node( - identifier, node, allocator, node_name, - node_namespace, no_demangle, retrieve_pub_cache, topic_names_and_types); + identifier, + node, + allocator, + node_name, + node_namespace, + _demangle_ros_topic_from_topic, + _demangle_if_ros_type, + no_demangle, + __get_reader_names_and_types_by_node, + topic_names_and_types); } -static rmw_ret_t -__get_service_names_and_types_by_node( +__rmw_get_publisher_names_and_types_by_node( const char * identifier, const rmw_node_t * node, rcutils_allocator_t * allocator, const char * node_name, const char * node_namespace, - rmw_names_and_types_t * service_names_and_types, - const char * topic_suffix) + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) { - const std::string topic_suffix_stdstr(topic_suffix); - rmw_ret_t valid_input = __validate_input( - identifier, node, allocator, node_name, node_namespace, service_names_and_types); - if (valid_input != RMW_RET_OK) { - return valid_input; - } - auto impl = static_cast(node->data); - __log_debug_information(*impl); - - GUID_t guid; - rmw_ret_t valid_guid = __get_guid_by_name(node, node_name, node_namespace, guid); - if (valid_guid != RMW_RET_OK) { - return valid_guid; - } - - std::map> services; - { - auto & topic_cache = impl->listener->reader_topic_cache; - std::lock_guard guard(topic_cache.getMutex()); - const auto & node_topics = topic_cache().getParticipantToTopics().find(guid); - if (node_topics != topic_cache().getParticipantToTopics().end()) { - for (auto & topic_pair : node_topics->second) { - std::string service_name = _demangle_service_from_topic(topic_pair.first); - if (service_name.empty()) { - // not a service - continue; - } - // Check if the topic suffix matches and is at the end of the name - const std::string & topic_name = topic_pair.first; - auto suffix_position = topic_name.rfind(topic_suffix_stdstr); - if (suffix_position == std::string::npos || - topic_name.length() - suffix_position - topic_suffix_stdstr.length() != 0) - { - continue; - } - - for (auto & itt : topic_pair.second) { - std::string service_type = _demangle_service_type_only(itt); - if (!service_type.empty()) { - services[service_name].insert(service_type); - } - } - } - } - } - if (services.empty()) { - return RMW_RET_OK; - } - // Setup string array to store names - rmw_ret_t rmw_ret = - rmw_names_and_types_init(service_names_and_types, services.size(), allocator); - if (rmw_ret != RMW_RET_OK) { - return rmw_ret; - } - // Setup cleanup function, in case of failure below - auto fail_cleanup = [&service_names_and_types]() { - rmw_ret_t rmw_ret = rmw_names_and_types_fini(service_names_and_types); - if (rmw_ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - kLoggerTag, - "error during report of error: %s", rmw_get_error_string().str); - } - }; - // For each service, store the name, initialize the string array for types, and store all types - size_t index = 0; - for (const auto & service_n_types : services) { - // Duplicate and store the service_name - char * service_name = rcutils_strdup(service_n_types.first.c_str(), *allocator); - if (!service_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for service name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - service_names_and_types->names.data[index] = service_name; - // Setup storage for types - rcutils_ret_t rcutils_ret = rcutils_string_array_init( - &service_names_and_types->types[index], - service_n_types.second.size(), - allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RMW_SET_ERROR_MSG(rcutils_get_error_string().str); - fail_cleanup(); - return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); - } - // Duplicate and store each type for the service - size_t type_index = 0; - for (const auto & type : service_n_types.second) { - char * type_name = rcutils_strdup(type.c_str(), *allocator); - if (!type_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for type name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - service_names_and_types->types[index].data[type_index] = type_name; - ++type_index; - } // for each type - ++index; - } // for each service - return RMW_RET_OK; + return __rmw_get_topic_names_and_types_by_node( + identifier, + node, + allocator, + node_name, + node_namespace, + _demangle_ros_topic_from_topic, + _demangle_if_ros_type, + no_demangle, + __get_writer_names_and_types_by_node, + topic_names_and_types); } rmw_ret_t @@ -514,14 +229,17 @@ __rmw_get_service_names_and_types_by_node( const char * node_namespace, rmw_names_and_types_t * service_names_and_types) { - return __get_service_names_and_types_by_node( + return __rmw_get_topic_names_and_types_by_node( identifier, node, allocator, node_name, node_namespace, - service_names_and_types, - "Request"); + _demangle_service_request_from_topic, + _demangle_service_type_only, + false, + __get_reader_names_and_types_by_node, + service_names_and_types); } rmw_ret_t @@ -533,14 +251,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..d8dd03b48 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,52 @@ __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; - } + return common_context->graph_cache.get_node_names( + node_names, + node_namespaces, + nullptr, + &allocator); +} - 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; +rmw_ret_t +__rmw_get_node_names_with_security_contexts( + const char * identifier, + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * security_contexts) +{ + if (!node) { + RMW_SET_ERROR_MSG("null node handle"); + return RMW_RET_ERROR; } - - 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; - } + if (rmw_check_zero_rmw_string_array(node_names) != RMW_RET_OK) { + return RMW_RET_ERROR; + } + if (rmw_check_zero_rmw_string_array(node_namespaces) != RMW_RET_OK) { + return RMW_RET_ERROR; } - 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 (rmw_check_zero_rmw_string_array(security_contexts) != RMW_RET_OK) { + return RMW_RET_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(); - } + if (node->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("node handle not from this implementation"); + return RMW_RET_ERROR; } - return RMW_RET_BAD_ALLOC; + + auto common_context = static_cast(node->context->impl->common); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + return common_context->graph_cache.get_node_names( + node_names, + node_namespaces, + security_contexts, + &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..99b45bac1 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) { @@ -54,37 +51,35 @@ __rmw_destroy_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_); + 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->listener_ != nullptr) { - delete info->listener_; - } - 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..ae5932b23 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) { @@ -57,38 +53,35 @@ __rmw_destroy_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"); + RMW_SET_ERROR_MSG("subscription handle not from this implementation"); return RMW_RET_ERROR; } - auto info = static_cast(subscription->data); - - if (info != nullptr) { - if (info->subscriber_ != nullptr) { - Domain::removeSubscriber(info->subscriber_); - } - if (info->listener_ != nullptr) { - delete info->listener_; + auto common_context = static_cast(node->context->impl->common); + auto info = static_cast(subscription->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( + info->subscription_gid_, common_context->gid, node->name, node->namespace_); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; } - if (info->type_support_ != nullptr) { - auto impl = static_cast(node->data); - if (!impl) { - RMW_SET_ERROR_MSG("node impl is null"); - return RMW_RET_ERROR; - } - - Participant * participant = impl->participant; - _unregister_type(participant, info->type_support_); - } - delete info; } - rmw_free(const_cast(subscription->topic_name)); - subscription->topic_name = nullptr; - rmw_subscription_free(subscription); - return RMW_RET_OK; + auto participant_info = + static_cast(node->context->impl->participant_info); + return destroy_subscription( + identifier, + participant_info, + subscription); } rmw_ret_t diff --git a/rmw_fastrtps_shared_cpp/src/rmw_topic_names_and_types.cpp b/rmw_fastrtps_shared_cpp/src/rmw_topic_names_and_types.cpp index 7d5da6414..761a73143 100644 --- a/rmw_fastrtps_shared_cpp/src/rmw_topic_names_and_types.cpp +++ b/rmw_fastrtps_shared_cpp/src/rmw_topic_names_and_types.cpp @@ -1,3 +1,4 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. // Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -12,36 +13,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include -#include -#include - #include "rcutils/allocator.h" -#include "rcutils/error_handling.h" -#include "rcutils/logging_macros.h" -#include "rcutils/strdup.h" -#include "rcutils/types.h" #include "rmw/allocators.h" -#include "rmw/convert_rcutils_ret_to_rmw_ret.h" #include "rmw/error_handling.h" #include "rmw/get_topic_names_and_types.h" #include "rmw/impl/cpp/macros.hpp" #include "rmw/names_and_types.h" -#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_dds_common/context.hpp" -#include "demangle.hpp" -#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" -#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" #include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" -#include "rmw_fastrtps_shared_cpp/topic_cache.hpp" +#include "demangle.hpp" namespace rmw_fastrtps_shared_cpp { + rmw_ret_t __rmw_get_topic_names_and_types( const char * identifier, @@ -58,108 +50,28 @@ __rmw_get_topic_names_and_types( RMW_SET_ERROR_MSG("null node handle"); return RMW_RET_INVALID_ARGUMENT; } - rmw_ret_t ret = rmw_names_and_types_check_zero(topic_names_and_types); if (ret != RMW_RET_OK) { return ret; } - - // Get participant pointer from node if (node->implementation_identifier != identifier) { RMW_SET_ERROR_MSG("node handle not from this implementation"); return RMW_RET_ERROR; } - auto impl = static_cast(node->data); - - // Access the slave Listeners, which are the ones that have the topicnamesandtypes member - // Get info from publisher and subscriber - // Combined results from the two lists - std::map> topics; - - // Setup processing function, will be used with two maps - auto map_process = - [&topics, no_demangle](const LockedObject & topic_cache) { - std::lock_guard guard(topic_cache.getMutex()); - for (auto it : topic_cache().getTopicToTypes()) { - if (!no_demangle && _get_ros_prefix_if_exists(it.first) != ros_topic_prefix) { - // if we are demangling and this is not prefixed with rt/, skip it - continue; - } - for (auto & itt : it.second) { - topics[it.first].insert(itt); - } - } - }; + DemangleFunction demangle_topic = _demangle_ros_topic_from_topic; + DemangleFunction demangle_type = _demangle_if_ros_type; - ::ParticipantListener * slave_target = impl->listener; - map_process(slave_target->reader_topic_cache); - map_process(slave_target->writer_topic_cache); - - // Copy data to results handle - if (!topics.empty()) { - // Setup string array to store names - rmw_ret_t rmw_ret = rmw_names_and_types_init(topic_names_and_types, topics.size(), allocator); - if (rmw_ret != RMW_RET_OK) { - return rmw_ret; - } - // Setup cleanup function, in case of failure below - auto fail_cleanup = [&topic_names_and_types]() { - rmw_ret_t rmw_ret = rmw_names_and_types_fini(topic_names_and_types); - if (rmw_ret != RMW_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - "rmw_fastrtps_shared_cpp", - "error during report of error: %s", rmw_get_error_string().str); - } - }; - // Setup demangling functions based on no_demangle option - auto demangle_topic = _demangle_if_ros_topic; - auto demangle_type = _demangle_if_ros_type; - if (no_demangle) { - auto noop = [](const std::string & in) { - return in; - }; - demangle_topic = noop; - demangle_type = noop; - } - // For each topic, store the name, initialize the string array for types, and store all types - size_t index = 0; - for (const auto & topic_n_types : topics) { - // Duplicate and store the topic_name - char * topic_name = rcutils_strdup(demangle_topic(topic_n_types.first).c_str(), *allocator); - if (!topic_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for topic name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - topic_names_and_types->names.data[index] = topic_name; - // Setup storage for types - { - rcutils_ret_t rcutils_ret = rcutils_string_array_init( - &topic_names_and_types->types[index], - topic_n_types.second.size(), - allocator); - if (rcutils_ret != RCUTILS_RET_OK) { - RMW_SET_ERROR_MSG(rcutils_get_error_string().str); - fail_cleanup(); - return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret); - } - } - // Duplicate and store each type for the topic - size_t type_index = 0; - for (const auto & type : topic_n_types.second) { - char * type_name = rcutils_strdup(demangle_type(type).c_str(), *allocator); - if (!type_name) { - RMW_SET_ERROR_MSG("failed to allocate memory for type name"); - fail_cleanup(); - return RMW_RET_BAD_ALLOC; - } - topic_names_and_types->types[index].data[type_index] = type_name; - ++type_index; - } // for each type - ++index; - } // for each topic + if (no_demangle) { + demangle_topic = _identity_demangle; + demangle_type = _identity_demangle; } - return RMW_RET_OK; + auto common_context = static_cast(node->context->impl->common); + + return common_context->graph_cache.get_names_and_types( + demangle_topic, + demangle_type, + allocator, + topic_names_and_types); } } // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/src/subscription.cpp b/rmw_fastrtps_shared_cpp/src/subscription.cpp new file mode 100644 index 000000000..1899bfe46 --- /dev/null +++ b/rmw_fastrtps_shared_cpp/src/subscription.cpp @@ -0,0 +1,78 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "fastrtps/Domain.h" +#include "fastrtps/TopicDataType.h" +#include "fastrtps/participant/Participant.h" +#include "fastrtps/subscriber/Subscriber.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +using Domain = eprosima::fastrtps::Domain; +using Participant = eprosima::fastrtps::Participant; +using TopicDataType = eprosima::fastrtps::TopicDataType; + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +destroy_subscription( + const char * identifier, + CustomParticipantInfo * participant_info, + rmw_subscription_t * subscription) +{ + if (!subscription) { + RMW_SET_ERROR_MSG("subscription handle is null"); + return RMW_RET_ERROR; + } + if (subscription->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("subscription handle not from this implementation"); + return RMW_RET_ERROR; + } + if (!participant_info) { + RMW_SET_ERROR_MSG("participant_info is null"); + return RMW_RET_ERROR; + } + + auto info = static_cast(subscription->data); + if (info != nullptr) { + if (info->subscriber_ != nullptr) { + Domain::removeSubscriber(info->subscriber_); + } + delete info->listener_; + if (info->type_support_ != nullptr) { + Participant * participant = participant_info->participant; + _unregister_type(participant, info->type_support_); + } + delete info; + } + rmw_free(const_cast(subscription->topic_name)); + subscription->topic_name = nullptr; + rmw_subscription_free(subscription); + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/rmw_fastrtps_shared_cpp/test/CMakeLists.txt b/rmw_fastrtps_shared_cpp/test/CMakeLists.txt index 868a5bcbf..5cb62c731 100644 --- a/rmw_fastrtps_shared_cpp/test/CMakeLists.txt +++ b/rmw_fastrtps_shared_cpp/test/CMakeLists.txt @@ -5,9 +5,3 @@ if(TARGET test_dds_attributes_to_rmw_qos) ament_target_dependencies(test_dds_attributes_to_rmw_qos) target_link_libraries(test_dds_attributes_to_rmw_qos ${PROJECT_NAME}) endif() - -ament_add_gtest(test_topic_cache test_topic_cache.cpp) -if(TARGET test_topic_cache) - ament_target_dependencies(test_topic_cache) - target_link_libraries(test_topic_cache ${PROJECT_NAME}) -endif() diff --git a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp b/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp deleted file mode 100644 index a93ba4de6..000000000 --- a/rmw_fastrtps_shared_cpp/test/test_topic_cache.cpp +++ /dev/null @@ -1,262 +0,0 @@ -// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include "gtest/gtest.h" - -#include "rmw_fastrtps_shared_cpp/topic_cache.hpp" - -#include "fastrtps/rtps/common/InstanceHandle.h" -#include "fastrtps/qos/ReaderQos.h" -#include "fastrtps/qos/WriterQos.h" -#include "rmw/types.h" - -using eprosima::fastrtps::WriterQos; -using eprosima::fastrtps::ReaderQos; -using eprosima::fastrtps::rtps::GUID_t; -using eprosima::fastrtps::rtps::GuidPrefix_t; -using eprosima::fastrtps::rtps::InstanceHandle_t; - -class TopicCacheTestFixture : public ::testing::Test -{ -public: - TopicCache topic_cache; - WriterQos qos[2]; - rmw_qos_profile_t rmw_qos[2]; - InstanceHandle_t participant_instance_handler[2]; - GUID_t participant_guid[2]; - GUID_t guid[2]; - - void SetUp() - { - // Create instance handlers - for (int i = 0; i < 2; i++) { - guid[i] = GUID_t(GuidPrefix_t(), i + 100); - participant_guid[i] = GUID_t(GuidPrefix_t(), i + 1); - participant_instance_handler[i] = participant_guid[i]; - } - - // Populating WriterQos -> which is from the DDS layer and - // rmw_qos_profile_t which is from rmw/types.h. - // This is done to test if topic_cache.getTopicNameToTopicData() returns - // the correct value in rmw_qos_profile_t for a given WriterQos - - // DDS qos - qos[0].m_durability.kind = eprosima::fastrtps::TRANSIENT_DURABILITY_QOS; - qos[0].m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; - qos[0].m_deadline.period = {123, 5678}; - qos[0].m_lifespan.duration = {190, 1234}; - qos[0].m_liveliness.kind = eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS; - qos[0].m_liveliness.lease_duration = {80, 5555555}; - - // equivalent rmq qos - rmw_qos[0].durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - rmw_qos[0].reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - rmw_qos[0].liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; - rmw_qos[0].liveliness_lease_duration.sec = 80u; - rmw_qos[0].liveliness_lease_duration.nsec = 5555555u; - rmw_qos[0].deadline.sec = 123u; - rmw_qos[0].deadline.nsec = 5678u; - rmw_qos[0].lifespan.sec = 190u; - rmw_qos[0].lifespan.nsec = 1234u; - - // DDS qos - qos[1].m_durability.kind = eprosima::fastrtps::TRANSIENT_DURABILITY_QOS; - qos[1].m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; - qos[1].m_deadline.period = {12, 1234}; - qos[1].m_lifespan.duration = {19, 5432}; - qos[1].m_liveliness.kind = eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS; - qos[1].m_liveliness.lease_duration = {8, 78901234}; - - // equivalent rmq qos - rmw_qos[1].durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; - rmw_qos[1].reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - rmw_qos[1].liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; - rmw_qos[1].liveliness_lease_duration.sec = 8u; - rmw_qos[1].liveliness_lease_duration.nsec = 78901234u; - rmw_qos[1].deadline.sec = 12u; - rmw_qos[1].deadline.nsec = 1234u; - rmw_qos[1].lifespan.sec = 19u; - rmw_qos[1].lifespan.nsec = 5432u; - - // Add data to topic_cache - topic_cache.addTopic(participant_instance_handler[0], guid[0], "topic1", "type1", qos[0]); - topic_cache.addTopic(participant_instance_handler[0], guid[0], "topic2", "type2", qos[0]); - topic_cache.addTopic(participant_instance_handler[1], guid[1], "topic1", "type1", qos[1]); - topic_cache.addTopic(participant_instance_handler[1], guid[1], "topic2", "type1", qos[1]); - } -}; - -TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_types) -{ - const auto topic_type_map = this->topic_cache.getTopicToTypes(); - - // topic1 - const auto & it = topic_type_map.find("topic1"); - ASSERT_TRUE(it != topic_type_map.end()); - // Verify that the object returned from the map is indeed the one expected - auto topic_types = it->second; - // Verify that there are two entries for type1 - EXPECT_EQ(std::count(topic_types.begin(), topic_types.end(), "type1"), 2); - EXPECT_EQ(topic_types.at(0), "type1"); - EXPECT_EQ(topic_types.at(1), "type1"); - - // topic2 - const auto & it2 = topic_type_map.find("topic2"); - ASSERT_TRUE(it2 != topic_type_map.end()); - // Verify that the object returned from the map is indeed the one expected - topic_types = it2->second; - // Verify that there are entries for type1 and type2 - EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type1") != topic_types.end()); - EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type2") != topic_types.end()); -} - -TEST_F(TopicCacheTestFixture, test_topic_cache_get_participant_map) -{ - const auto & participant_topic_map = this->topic_cache.getParticipantToTopics(); - - // participant 1 - const auto & it = participant_topic_map.find(this->participant_guid[0]); - ASSERT_TRUE(it != participant_topic_map.end()); - // Verify that the topic and respective types are present - const auto & topic_type_map = it->second; - - const auto & topic1it = topic_type_map.find("topic1"); - ASSERT_TRUE(topic1it != topic_type_map.end()); - auto topic_types = topic1it->second; - EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type1") != topic_types.end()); - - const auto & topic2it = topic_type_map.find("topic2"); - ASSERT_TRUE(topic2it != topic_type_map.end()); - topic_types = topic2it->second; - EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type2") != topic_types.end()); - - // participant 2 - const auto & it2 = participant_topic_map.find(this->participant_guid[1]); - ASSERT_TRUE(it2 != participant_topic_map.end()); - // Verify that the topic and respective types are present - const auto & topic_type_map2 = it2->second; - - const auto & topic1it2 = topic_type_map2.find("topic1"); - ASSERT_TRUE(topic1it2 != topic_type_map2.end()); - topic_types = topic1it2->second; - EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type1") != topic_types.end()); - - const auto & topic2it2 = topic_type_map2.find("topic2"); - ASSERT_TRUE(topic2it2 != topic_type_map2.end()); - topic_types = topic2it2->second; - EXPECT_TRUE(std::find(topic_types.begin(), topic_types.end(), "type1") != topic_types.end()); -} - -TEST_F(TopicCacheTestFixture, test_topic_cache_get_topic_name_topic_data_map) -{ - const auto & topic_data_map = this->topic_cache.getTopicNameToTopicData(); - auto expected_results = std::map>(); - expected_results["topic1"].push_back({participant_guid[0], guid[0], "type1", rmw_qos[0]}); - expected_results["topic1"].push_back({participant_guid[1], guid[1], "type1", rmw_qos[1]}); - expected_results["topic2"].push_back({participant_guid[0], guid[0], "type2", rmw_qos[0]}); - expected_results["topic2"].push_back({participant_guid[1], guid[1], "type1", rmw_qos[1]}); - for (const auto & result_it : expected_results) { - const auto & topic_name = result_it.first; - const auto & expected_topic_data = result_it.second; - - const auto & it = topic_data_map.find(topic_name); - ASSERT_TRUE(it != topic_data_map.end()); - // Verify that the topic has all the associated data - const auto & topic_data = it->second; - for (auto i = 0u; i < expected_topic_data.size(); i++) { - // PARTICIPANT GUID - EXPECT_EQ(topic_data.at(i).participant_guid, expected_topic_data.at(i).participant_guid); - // GUID - EXPECT_EQ(topic_data.at(i).entity_guid, expected_topic_data.at(i).entity_guid); - // TYPE - EXPECT_EQ(topic_data.at(i).topic_type, expected_topic_data.at(i).topic_type); - // QOS - const auto & qos = topic_data.at(i).qos_profile; - const auto & expected_qos = expected_topic_data.at(i).qos_profile; - EXPECT_EQ(qos.durability, expected_qos.durability); - EXPECT_EQ(qos.reliability, expected_qos.reliability); - EXPECT_EQ(qos.liveliness, expected_qos.liveliness); - EXPECT_EQ(qos.liveliness_lease_duration.sec, expected_qos.liveliness_lease_duration.sec); - EXPECT_EQ(qos.liveliness_lease_duration.nsec, expected_qos.liveliness_lease_duration.nsec); - EXPECT_EQ(qos.deadline.sec, expected_qos.deadline.sec); - EXPECT_EQ(qos.deadline.nsec, expected_qos.deadline.nsec); - EXPECT_EQ(qos.lifespan.sec, expected_qos.lifespan.sec); - EXPECT_EQ(qos.lifespan.nsec, expected_qos.lifespan.nsec); - } - } -} - -TEST_F(TopicCacheTestFixture, test_topic_cache_add_topic) -{ - // Add Topic - const bool did_add = this->topic_cache.addTopic( - this->participant_instance_handler[1], this->guid[1], "TestTopic", "TestType", this->qos[1]); - // Verify that the returned value was true - EXPECT_TRUE(did_add); -} - -TEST_F(TopicCacheTestFixture, test_topic_cache_remove_topic_element_exists) -{ - auto did_remove = this->topic_cache.removeTopic( - this->participant_instance_handler[0], this->guid[0], "topic1", "type1"); - // Assert that the return was true - ASSERT_TRUE(did_remove); - // Verify it is removed from TopicToTypes - const auto & topic_type_map = this->topic_cache.getTopicToTypes(); - const auto & topic_type_it = topic_type_map.find("topic1"); - ASSERT_TRUE(topic_type_it != topic_type_map.end()); - EXPECT_EQ(std::count(topic_type_it->second.begin(), topic_type_it->second.end(), "type1"), 1); - // Verify it is removed from ParticipantTopicMap - const auto & participant_topic_map = this->topic_cache.getParticipantToTopics(); - const auto & participant_topic_it = participant_topic_map.find(this->participant_guid[0]); - ASSERT_TRUE(participant_topic_it != participant_topic_map.end()); - const auto & p_topic_type_it = participant_topic_it->second.find("topic1"); - ASSERT_TRUE(p_topic_type_it == participant_topic_it->second.end()); - // Verify it is removed from TopicNameToTopicTypeMap - const auto & topic_data_map = this->topic_cache.getTopicNameToTopicData(); - const auto & topic_data_it = topic_data_map.find("topic1"); - ASSERT_TRUE(topic_data_it != topic_data_map.end()); - EXPECT_EQ(topic_data_it->second.size(), 1u); - - did_remove = this->topic_cache.removeTopic( - this->participant_instance_handler[1], this->guid[1], "topic1", "type1"); - ASSERT_TRUE(did_remove); - const auto & topic_type_map2 = this->topic_cache.getTopicToTypes(); - const auto & topic_type_it2 = topic_type_map2.find("topic1"); - ASSERT_TRUE(topic_type_it2 == topic_type_map2.end()); - const auto & participant_topic_map2 = this->topic_cache.getParticipantToTopics(); - const auto & participant_topic_it2 = participant_topic_map2.find(this->participant_guid[1]); - ASSERT_TRUE(participant_topic_it2 != participant_topic_map2.end()); - const auto & p_topic_type_it2 = participant_topic_it2->second.find("topic1"); - ASSERT_TRUE(p_topic_type_it2 == participant_topic_it2->second.end()); - const auto & topic_data_map2 = this->topic_cache.getTopicNameToTopicData(); - const auto & topic_data_it2 = topic_data_map2.find("topic1"); - ASSERT_TRUE(topic_data_it2 == topic_data_map2.end()); -} - -TEST_F(TopicCacheTestFixture, test_topic_cache_remove_policy_element_does_not_exist) -{ - // add topic - this->topic_cache.addTopic( - this->participant_instance_handler[1], this->guid[1], "TestTopic", "TestType", this->qos[1]); - // Assert that the return was false - const auto did_remove = this->topic_cache.removeTopic( - this->participant_instance_handler[1], this->guid[1], "NewTestTopic", "TestType"); - ASSERT_FALSE(did_remove); -}