From 5cb1b535a984ce72ee765fa6d48e881cb6df751c Mon Sep 17 00:00:00 2001 From: Timon Engelke Date: Sat, 21 May 2022 21:07:03 +0200 Subject: [PATCH] Import Upstream version 6.2.1 --- CHANGELOG.rst | 287 +++++ CMakeLists.txt | 182 ++++ Doxyfile | 30 + QUALITY_DECLARATION.md | 204 ++++ README.md | 9 + .../MessageTypeSupport.hpp | 42 + .../MessageTypeSupport_impl.hpp | 69 ++ .../ServiceTypeSupport.hpp | 47 + .../ServiceTypeSupport_impl.hpp | 101 ++ .../rmw_fastrtps_dynamic_cpp/TypeSupport.hpp | 201 ++++ .../TypeSupport_impl.hpp | 977 ++++++++++++++++++ .../rmw_fastrtps_dynamic_cpp/get_client.hpp | 51 + .../get_participant.hpp | 39 + .../get_publisher.hpp | 39 + .../rmw_fastrtps_dynamic_cpp/get_service.hpp | 51 + .../get_subscriber.hpp | 39 + .../rmw_fastrtps_dynamic_cpp/identifier.hpp | 20 + .../init_rmw_context_impl.hpp | 33 + include/rmw_fastrtps_dynamic_cpp/macros.hpp | 36 + .../rmw_fastrtps_dynamic_cpp/publisher.hpp | 36 + .../serialization_format.hpp | 20 + .../rmw_fastrtps_dynamic_cpp/subscription.hpp | 38 + .../visibility_control.h | 56 + package.xml | 57 + rmw_fastrtps_dynamic_cpp-extras.cmake | 24 + src/client_service_common.cpp | 46 + src/client_service_common.hpp | 46 + src/get_client.cpp | 49 + src/get_participant.cpp | 39 + src/get_publisher.cpp | 36 + src/get_service.cpp | 49 + src/get_subscriber.cpp | 36 + src/identifier.cpp | 17 + src/init_rmw_context_impl.cpp | 203 ++++ src/publisher.cpp | 332 ++++++ src/rmw_client.cpp | 575 +++++++++++ src/rmw_compare_gids_equal.cpp | 31 + src/rmw_count.cpp | 50 + src/rmw_event.cpp | 50 + src/rmw_features.cpp | 27 + src/rmw_get_endpoint_network_flow.cpp | 46 + src/rmw_get_gid_for_publisher.cpp | 31 + src/rmw_get_implementation_identifier.cpp | 26 + src/rmw_get_serialization_format.cpp | 26 + src/rmw_get_topic_endpoint_info.cpp | 46 + src/rmw_guard_condition.cpp | 45 + src/rmw_init.cpp | 167 +++ src/rmw_logging.cpp | 29 + src/rmw_node.cpp | 123 +++ src/rmw_node_info_and_types.cpp | 100 ++ src/rmw_node_names.cpp | 53 + src/rmw_publish.cpp | 57 + src/rmw_publisher.cpp | 222 ++++ src/rmw_qos.cpp | 30 + src/rmw_request.cpp | 45 + src/rmw_response.cpp | 44 + src/rmw_serialize.cpp | 105 ++ src/rmw_service.cpp | 574 ++++++++++ src/rmw_service_names_and_types.cpp | 37 + src/rmw_service_server_is_available.cpp | 36 + src/rmw_subscription.cpp | 212 ++++ src/rmw_take.cpp | 131 +++ src/rmw_topic_names_and_types.cpp | 49 + src/rmw_trigger_guard_condition.cpp | 30 + src/rmw_wait.cpp | 37 + src/rmw_wait_set.cpp | 38 + src/serialization_format.cpp | 17 + src/subscription.cpp | 361 +++++++ src/type_support_common.cpp | 36 + src/type_support_common.hpp | 112 ++ src/type_support_proxy.cpp | 49 + src/type_support_registry.cpp | 158 +++ src/type_support_registry.hpp | 74 ++ test/test_get_native_entities.cpp | 173 ++++ test/test_logging.cpp | 47 + 75 files changed, 7640 insertions(+) create mode 100644 CHANGELOG.rst create mode 100644 CMakeLists.txt create mode 100644 Doxyfile create mode 100644 QUALITY_DECLARATION.md create mode 100644 README.md create mode 100644 include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/get_client.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/get_participant.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/get_service.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/identifier.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/macros.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/publisher.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/serialization_format.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/subscription.hpp create mode 100644 include/rmw_fastrtps_dynamic_cpp/visibility_control.h create mode 100644 package.xml create mode 100644 rmw_fastrtps_dynamic_cpp-extras.cmake create mode 100644 src/client_service_common.cpp create mode 100644 src/client_service_common.hpp create mode 100644 src/get_client.cpp create mode 100644 src/get_participant.cpp create mode 100644 src/get_publisher.cpp create mode 100644 src/get_service.cpp create mode 100644 src/get_subscriber.cpp create mode 100644 src/identifier.cpp create mode 100644 src/init_rmw_context_impl.cpp create mode 100644 src/publisher.cpp create mode 100644 src/rmw_client.cpp create mode 100644 src/rmw_compare_gids_equal.cpp create mode 100644 src/rmw_count.cpp create mode 100644 src/rmw_event.cpp create mode 100644 src/rmw_features.cpp create mode 100644 src/rmw_get_endpoint_network_flow.cpp create mode 100644 src/rmw_get_gid_for_publisher.cpp create mode 100644 src/rmw_get_implementation_identifier.cpp create mode 100644 src/rmw_get_serialization_format.cpp create mode 100644 src/rmw_get_topic_endpoint_info.cpp create mode 100644 src/rmw_guard_condition.cpp create mode 100644 src/rmw_init.cpp create mode 100644 src/rmw_logging.cpp create mode 100644 src/rmw_node.cpp create mode 100644 src/rmw_node_info_and_types.cpp create mode 100644 src/rmw_node_names.cpp create mode 100644 src/rmw_publish.cpp create mode 100644 src/rmw_publisher.cpp create mode 100644 src/rmw_qos.cpp create mode 100644 src/rmw_request.cpp create mode 100644 src/rmw_response.cpp create mode 100644 src/rmw_serialize.cpp create mode 100644 src/rmw_service.cpp create mode 100644 src/rmw_service_names_and_types.cpp create mode 100644 src/rmw_service_server_is_available.cpp create mode 100644 src/rmw_subscription.cpp create mode 100644 src/rmw_take.cpp create mode 100644 src/rmw_topic_names_and_types.cpp create mode 100644 src/rmw_trigger_guard_condition.cpp create mode 100644 src/rmw_wait.cpp create mode 100644 src/rmw_wait_set.cpp create mode 100644 src/serialization_format.cpp create mode 100644 src/subscription.cpp create mode 100644 src/type_support_common.cpp create mode 100644 src/type_support_common.hpp create mode 100644 src/type_support_proxy.cpp create mode 100644 src/type_support_registry.cpp create mode 100644 src/type_support_registry.hpp create mode 100644 test/test_get_native_entities.cpp create mode 100644 test/test_logging.cpp diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..868a55b --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,287 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rmw_fastrtps_dynamic_cpp +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +6.2.1 (2022-03-28) +------------------ +* Add content filter topic feature (`#513 `_) +* Add sequence numbers to message info structure (`#587 `_) +* Contributors: Chen Lihui, Ivan Santiago Paunovic + +6.2.0 (2022-03-01) +------------------ +* Add EventsExecutor (`#468 `_) +* Install headers to include/${PROJECT_NAME} (`#578 `_) +* Contributors: Shane Loretz, iRobot ROS + +6.1.2 (2022-01-14) +------------------ + +6.1.1 (2021-12-17) +------------------ + +6.1.0 (2021-11-19) +------------------ +* Add client/service QoS getters. (`#560 `_) +* Contributors: mauropasse + +6.0.0 (2021-09-15) +------------------ + +5.2.2 (2021-08-09) +------------------ +* Correctly recalculate serialized size on bounded sequences. (`#540 `_) +* Fix type size alignment. (`#550 `_) +* Contributors: Miguel Company + +5.2.1 (2021-06-30) +------------------ + +5.2.0 (2021-06-04) +------------------ +* Add rmw_publisher_wait_for_all_acked support. (`#519 `_) +* Contributors: Barry Xu + +5.1.0 (2021-05-12) +------------------ +* Loan messages implementation (`#523 `_) + * Added is_plain\_ attribute to base TypeSupport. + * Added new methods to base TypeSupport. + * Implementation of rmw_borrow_loaned_message. + * Implementation of rmw_return_loaned_message_from_publisher. + * Enable loan messages on publishers of plain types. + * Implementation for taking loaned messages. + * Enable loan messages on subscriptions of plain types. +* Contributors: Miguel Company + +5.0.0 (2021-04-06) +------------------ +* Refactor to use DDS standard API (`#518 `_) +* Unique network flows (`#502 `_) +* updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#520 `_) +* Contributors: Miguel Company, shonigmann + +4.5.0 (2021-03-18) +------------------ + +4.4.0 (2021-03-01) +------------------ +* Add RMW function to check QoS compatibility (`#511 `_) +* Capture cdr exceptions (`#505 `_) +* Load profiles based on topic names in rmw_fastrtps_dynamic_cpp (`#497 `_) +* Contributors: Eduardo Ponz Segrelles, Jacob Perron, Miguel Company + +4.3.0 (2021-01-25) +------------------ +* Set rmw_dds_common::GraphCache callback after init succeeds. (`#496 `_) +* Handle typesupport errors on fetch. (`#495 `_) +* Contributors: Michel Hidalgo + +4.2.0 (2020-12-10) +------------------ + +4.1.0 (2020-12-08) +------------------ +* Check for correct context shutdown (`#486 `_) +* New environment variable to change easily the publication mode (`#470 `_) +* Contributors: Ignacio Montesino Valle, José Luis Bueno López + +4.0.0 (2020-10-22) +------------------ +* Discriminate when the Client has gone from when the Client has not completely matched (`#467 `_) + * Workaround when the client is gone before server sends response + * Change add to the map to listener callback +* Update the package.xml files with the latest Open Robotics maintainers (`#459 `_) +* Update Quality Declarations and READMEs (`#455 `_) + * Add QL of external dependencies to rmw_fastrtps_dynamic_cpp QD + * Add QD links for dependencies to rmw_fastrtps_dynamic_cpp QD + * Provide external dependencies QD links + * Add README to rmw_fastrtps_dynamic + * Add QD for rmw_fastrtps_dynamic +* Contributors: JLBuenoLopez-eProsima, Jaime Martin Losa, José Luis Bueno López, Michael Jeronimo + +3.1.4 (2020-10-02) +------------------ +* Ensure rmw_destroy_node() completes despite run-time errors. (`#458 `_) +* Contributors: Michel Hidalgo + +3.1.3 (2020-09-29) +------------------ +* Return RMW_RET_UNSUPPORTED in rmw_get_serialized_message_size (`#452 `_) +* Contributors: Alejandro Hernández Cordero + +3.1.2 (2020-09-25) +------------------ +* Updated publisher/subscription allocation and wait set API return codes (`#443 `_) +* Added rmw_logging tests (`#442 `_) +* Contributors: Alejandro Hernández Cordero + +3.1.1 (2020-09-24) +------------------ +* Fix array `get_function` semantics (`#448 `_) +* Make service/client construction/destruction implementation compliant (`#445 `_) +* Make sure type can be unregistered successfully (`#437 `_) +* Contributors: Barry Xu, Ivan Santiago Paunovic, Michel Hidalgo + +3.1.0 (2020-09-23) +------------------ +* Add tests for native entity getters. (`#439 `_) +* Avoid deadlock if graph update fails. (`#438 `_) +* Contributors: Michel Hidalgo + +3.0.0 (2020-09-18) +------------------ +* Call Domain::removePublisher while failure occurs in create_publisher (`#434 `_) +* Avoid memory leaks and undefined behavior in rmw_fastrtps_dynamic_cpp typesupport code (`#429 `_) +* Contributors: Barry Xu, Miguel Company + +2.6.0 (2020-08-28) +------------------ +* Ensure compliant matched pub/sub count API. (`#424 `_) +* Ensure compliant publisher QoS queries. (`#425 `_) +* Contributors: Michel Hidalgo + +2.5.0 (2020-08-07) +------------------ + +2.4.0 (2020-08-06) +------------------ +* Ensure compliant subscription API. (`#419 `_) +* Contributors: Michel Hidalgo + +2.3.0 (2020-07-30) +------------------ +* Ensure compliant publisher API. (`#414 `_) +* Contributors: Michel Hidalgo + +2.2.0 (2020-07-22) +------------------ +* Set context actual domain id (`#410 `_) +* Contributors: Ivan Santiago Paunovic + +2.1.0 (2020-07-20) +------------------ +* Ensure compliant node construction/destruction API. (`#408 `_) +* Contributors: Michel Hidalgo + +2.0.0 (2020-07-08) +------------------ +* Remove domain_id and localhost_only from node API (`#407 `_) +* Amend rmw_init() implementation: require enclave. (`#406 `_) +* Contributors: Ivan Santiago Paunovic, Michel Hidalgo + +1.1.0 (2020-06-29) +------------------ +* Ensure compliant init/shutdown API implementation. (`#401 `_) +* Finalize context iff shutdown. (`#396 `_) +* Make service wait for response reader (`#390 `_) +* Contributors: Michel Hidalgo, Miguel Company + +1.0.1 (2020-06-01) +------------------ + +1.0.0 (2020-05-12) +------------------ +* Fix single rmw build for rmw_fastrtps_dynamic_cpp (`#381 `_) +* Remove API related to manual by node liveliness (`#379 `_) +* Contributors: Ivan Santiago Paunovic + +0.9.1 (2020-05-08) +------------------ +* Added doxyfiles (`#372 `_) +* Contributors: Alejandro Hernández Cordero + +0.9.0 (2020-04-28) +------------------ +* Fixed rmw_fastrtps_dynamic_cpp package description. (`#376 `_) +* Rename rosidl_message_bounds_t. (`#373 `_) +* Feature/services timestamps. (`#369 `_) +* Add support for taking a sequence of messages. (`#366 `_) +* security-context -> enclave. (`#365 `_) +* Rename rosidl_generator_c namespace to rosidl_runtime_c. (`#367 `_) +* Remove custom typesupport for rmw_dds_common interfaces. (`#364 `_) +* Added rosidl_runtime c and cpp depencencies. (`#351 `_) +* Switch to one Participant per Context. (`#312 `_) +* Add rmw\_*_event_init() functions. (`#354 `_) +* Fixing type support C/CPP mix on rmw_fastrtps_dynamic_cpp. (`#350 `_) +* Fix build warning in Ubuntu Focal. (`#346 `_) +* Code style only: wrap after open parenthesis if not in one line. (`#347 `_) +* Passing down type support information (`#342 `_) +* Implement functions to get publisher and subcription informations like QoS policies from topic name. (`#336 `_) +* Contributors: Alejandro Hernández Cordero, Dirk Thomas, Ingo Lütkebohle, Ivan Santiago Paunovic, Jaison Titus, Miaofei Mei, Michael Carroll, Miguel Company, Mikael Arguedas + +0.8.1 (2019-10-23) +------------------ +* use return_loaned_message_from (`#334 `_) +* Restrict traffic to localhost only if env var is provided (`#331 `_) +* Zero copy api (`#322 `_) +* update signature for added pub/sub options (`#329 `_) +* Contributors: Brian Marchi, Karsten Knese, William Woodall + +0.8.0 (2019-09-25) +------------------ +* Add function for getting clients by node (`#293 `_) +* Use rcpputils::find_and_replace instead of std::regex_replace (`#291 `_) +* Export typesupport_fastrtps package dependencies (`#294 `_) +* Implement get_actual_qos() for subscriptions (`#287 `_) +* Contributors: Jacob Perron, M. M, kurcha01-arm + +0.7.3 (2019-05-29) +------------------ + +0.7.2 (2019-05-20) +------------------ +* add support for WString in rmw_fastrtps_dynamic_cpp (`#278 `_) +* Centralize topic name creation logic and update to match FastRTPS 1.8 API (`#272 `_) +* Contributors: Dirk Thomas, Nick Burek + +0.7.1 (2019-05-08) +------------------ +* Support arbitrary message namespaces (`#266 `_) +* Add qos interfaces with no-op (`#271 `_) +* Updates for preallocation API. (`#274 `_) +* Contributors: Jacob Perron, Michael Carroll, Ross Desmond + +0.7.0 (2019-04-13) +------------------ +* Add function to get publisher actual qos settings (`#267 `_) +* pass context to wait set and fini context (`#252 `_) +* Add missing logic to dynamic RMW client implementation (`#254 `_) +* Merge pull request `#250 `_ from ros2/support_static_lib +* use namespace_prefix from shared package +* Use empty() instead of size() to check if a vector/map contains elements and fixed some incorrect logging (`#245 `_) +* Contributors: Dirk Thomas, Jacob Perron, Johnny Willemsen, William Woodall, ivanpauno + +0.6.1 (2018-12-06) +------------------ +* Add topic cache object for managing topic relations (`#236 `_) +* Fastrtps 1.7.0 (`#233 `_) +* RMW_FastRTPS configuration from XML only (`#243 `_) +* refactor to support init options and context (`#237 `_) +* Methods to retrieve matched counts on pub/sub (`#234 `_) +* Fixing failing tests on rmw_fastrtps_dynamic_cpp. (`#242 `_) +* use uint8_array (`#240 `_) +* fix linter warnings (`#241 `_) +* Contributors: Dirk Thomas, Juan Carlos, Karsten Knese, Michael Carroll, MiguelCompany, Ross Desmond, William Woodall + +0.6.0 (2018-11-16) +------------------ +* Merge pull request `#232 `_ from ros2/array-terminology +* rename files +* rename dynamic array to sequence +* Add semicolons to all RCLCPP and RCUTILS macros. (`#229 `_) +* Include node namespaces in get_node_names (`#224 `_) +* add rmw_get_serialization_format (`#215 `_) +* Merge pull request `#218 `_ from ros2/pr203 +* Refs `#3061 `_. Adapting code on rmw_fastrtps_dynamic_cpp. +* Refs `#3061 `_. Package rmw_fastrtps_cpp duplicated as rmw_fastrtps_dynamic_cpp. +* Contributors: Chris Lalancette, Dirk Thomas, Karsten Knese, Michael Carroll, Miguel Company + +0.5.1 (2018-06-28) +------------------ + +0.5.0 (2018-06-23) +------------------ + +0.4.0 (2017-12-08) +------------------ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..3c34ef7 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,182 @@ +# 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. + +cmake_minimum_required(VERSION 3.5) + +project(rmw_fastrtps_dynamic_cpp) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +if(SECURITY) + find_package(OpenSSL REQUIRED) +endif() + +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) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps 2.3 REQUIRED CONFIG) +find_package(FastRTPS 2.3 REQUIRED MODULE) + +find_package(rmw REQUIRED) +find_package(rosidl_runtime_c REQUIRED) +find_package(rosidl_typesupport_fastrtps_c REQUIRED) +find_package(rosidl_typesupport_fastrtps_cpp REQUIRED) +find_package(rosidl_typesupport_introspection_c REQUIRED) +find_package(rosidl_typesupport_introspection_cpp REQUIRED) + +include_directories(include) + +add_library(rmw_fastrtps_dynamic_cpp + src/client_service_common.cpp + src/get_client.cpp + src/get_participant.cpp + src/get_publisher.cpp + src/get_service.cpp + src/get_subscriber.cpp + src/identifier.cpp + src/init_rmw_context_impl.cpp + src/publisher.cpp + src/rmw_client.cpp + src/rmw_compare_gids_equal.cpp + src/rmw_count.cpp + src/rmw_event.cpp + src/rmw_features.cpp + src/rmw_get_gid_for_publisher.cpp + src/rmw_get_implementation_identifier.cpp + src/rmw_get_serialization_format.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 + src/rmw_publish.cpp + src/rmw_publisher.cpp + src/rmw_qos.cpp + src/rmw_request.cpp + src/rmw_response.cpp + src/rmw_serialize.cpp + src/rmw_service.cpp + src/rmw_service_names_and_types.cpp + src/rmw_service_server_is_available.cpp + src/rmw_subscription.cpp + src/rmw_take.cpp + src/rmw_topic_names_and_types.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/rmw_get_endpoint_network_flow.cpp +) +target_link_libraries(rmw_fastrtps_dynamic_cpp + fastcdr fastrtps) + +# specific order: dependents before dependencies +ament_target_dependencies(rmw_fastrtps_dynamic_cpp + "rcpputils" + "rcutils" + "rosidl_typesupport_fastrtps_c" + "rosidl_typesupport_fastrtps_cpp" + "rosidl_typesupport_introspection_c" + "rosidl_typesupport_introspection_cpp" + "rmw_dds_common" + "rmw_fastrtps_shared_cpp" + "rmw" + "rosidl_runtime_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, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} +PRIVATE "RMW_FASTRTPS_DYNAMIC_CPP_BUILDING_LIBRARY") + +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(rmw_fastrtps_dynamic_cpp) + +# specific order: dependents before dependencies +ament_export_dependencies(rcpputils) +ament_export_dependencies(rcutils) +ament_export_dependencies(rmw) +ament_export_dependencies(rmw_dds_common) +ament_export_dependencies(rmw_fastrtps_shared_cpp) +ament_export_dependencies(rosidl_runtime_c) +ament_export_dependencies(rosidl_typesupport_fastrtps_c) +ament_export_dependencies(rosidl_typesupport_fastrtps_cpp) +ament_export_dependencies(rosidl_typesupport_introspection_c) +ament_export_dependencies(rosidl_typesupport_introspection_cpp) + +register_rmw_implementation( + "c:rosidl_typesupport_c:rosidl_typesupport_introspection_c" + "cpp:rosidl_typesupport_cpp:rosidl_typesupport_introspection_cpp") + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + find_package(osrf_testing_tools_cpp REQUIRED) + find_package(test_msgs REQUIRED) + + ament_add_gtest(test_get_native_entities + test/test_get_native_entities.cpp) + ament_target_dependencies(test_get_native_entities + osrf_testing_tools_cpp rcutils rmw test_msgs + ) + target_link_libraries(test_get_native_entities rmw_fastrtps_dynamic_cpp) + + ament_add_gtest(test_logging test/test_logging.cpp) + ament_target_dependencies(test_logging rmw) + target_link_libraries(test_logging rmw_fastrtps_dynamic_cpp) +endif() + +ament_package( + CONFIG_EXTRAS_POST "rmw_fastrtps_dynamic_cpp-extras.cmake" +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install( + TARGETS rmw_fastrtps_dynamic_cpp + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) diff --git a/Doxyfile b/Doxyfile new file mode 100644 index 0000000..5d6a2fe --- /dev/null +++ b/Doxyfile @@ -0,0 +1,30 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "rmw_fastrtps_dynamic_cpp" +PROJECT_NUMBER = master +PROJECT_BRIEF = "Implement the ROS middleware interface using eProsima FastRTPS dynamic code generation in C++." + +INPUT = ./include + +RECURSIVE = YES +OUTPUT_DIRECTORY = doc_output + +EXTRACT_ALL = YES +SORT_MEMBER_DOCS = NO + +GENERATE_LATEX = NO + +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +PREDEFINED = RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC= + +# Tag files that do not exist will produce a warning and cross-project linking will not work. +TAGFILES += "../../../../doxygen_tag_files/cppreference-doxygen-web.tag.xml=http://en.cppreference.com/w/" +# Consider changing "latest" to the version you want to reference (e.g. beta1 or 1.0.0) +TAGFILES += "../../../../doxygen_tag_files/rcl.tag=http://docs.ros2.org/latest/api/rcl/" +TAGFILES += "../../../../doxygen_tag_files/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +TAGFILES += "../../../../doxygen_tag_files/rmw_fastrtps_shared_cpp.tag=http://docs.ros2.org/latest/api/rmw_fastrtps_shared_cpp/" +TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +# Uncomment to generate tag files for cross-project linking. +GENERATE_TAGFILE = "../../../../doxygen_tag_files/rmw_fastrtps_dynamic_cpp.tag" diff --git a/QUALITY_DECLARATION.md b/QUALITY_DECLARATION.md new file mode 100644 index 0000000..073bec1 --- /dev/null +++ b/QUALITY_DECLARATION.md @@ -0,0 +1,204 @@ +This document is a declaration of software quality for the `rmw_fastrtps_dynamic_cpp` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). + +# `rmw_fastrtps_dynamic_cpp` Quality Declaration + +The package `rmw_fastrtps_dynamic_cpp` claims to be in the **Quality Level 3** category. + +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 3 in REP-2004](https://www.ros.org/reps/rep-2004.html). + +## Version Policy [1] + +### Version Scheme [1.i] + +`rmw_fastrtps_dynamic_cpp` uses `semver` according to the recommendation for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#versioning). + +### Version Stability [1.ii] + +`rmw_fastrtps_dynamic_cpp` is at a stable version, i.e. `>= 1.0.0`. +The current version can be found in its [package.xml](package.xml), and its change history can be found in its [CHANGELOG](CHANGELOG.rst). + +### Public API Declaration [1.iii] + +All symbols in the installed headers are considered part of the public API. + +All installed headers are in the `include` directory of the package, headers in any other folders are not installed and considered private. + +### API Stability Within a Released ROS Distribution [1.iv]/[1.vi] + +`rmw_fastrtps_dynamic_cpp` will not break public API within a released ROS distribution, i.e. no major releases once the ROS distribution is released. + +### ABI Stability Within a Released ROS Distribution [1.v]/[1.vi] + +`rmw_fastrtps_dynamic_cpp` contains C and C++ code and therefore must be concerned with ABI stability, and will maintain ABI stability within a ROS distribution. + +## Change Control Process [2] + +`rmw_fastrtps_dynamic_cpp` follows the recommended guidelines for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#quality-practices). + +### Change Requests [2.i] + +This package requires that all changes occur through a pull request. + +### Contributor Origin [2.ii] + +This package uses DCO as its confirmation of contributor origin policy. More information can be found in [CONTRIBUTING](../CONTRIBUTING.md). + +### Peer Review Policy [2.iii] + +Following the recommended guidelines for ROS Core packages, all pull requests must have at least 1 peer review. +Check [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process) for additional information. + +### Continuous Integration [2.iv] + +All pull request must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers). + +### Documentation Policy [2.v] + +All pull requests must resolve related documentation changes before merging. + +## Documentation [3] + +### Feature Documentation [3.i] + +Some of the `rmw_fastrtps_dynamic_cpp` features are documented on the repository level [README](../README.md). +Much of Fast DDS itself has feature documentation [hosted publicly](https://fast-dds.docs.eprosima.com/en/latest/). + +### Public API Documentation [3.ii] + +Most of `rmw_fastrtps_dynamic_cpp` has embedded API documentation. It is not yet hosted publicly. + +### License [3.iii] + +The license for `rmw_fastrtps_dynamic_cpp` is Apache 2.0, and a summary is in each source file, the type is declared in the [package.xml](package.xml) manifest file, and a full copy of the license is in the [LICENSE](../LICENSE) file. + +There is an automated test which runs a linter that ensures each file has a license statement. + +Most recent test results can be found [here](https://ci.ros2.org/view/nightly/job/nightly_linux_extra_rmw_release/lastCompletedBuild/testReport/rmw_fastrtps_dynamic_cpp/copyright/) + +### Copyright Statements [3.iv] + +The copyright holders each provide a statement of copyright in each source code file in `rmw_fastrtps_dynamic_cpp`. + +There is an automated test which runs a linter that ensures each file has at least one copyright statement. + +The results of the test can be found [here](https://ci.ros2.org/view/nightly/job/nightly_linux_extra_rmw_release/lastCompletedBuild/testReport/rmw_fastrtps_dynamic_cpp/copyright/). + +## Testing [4] + +### Feature Testing [4.i] + +All `rmw_fastrtps_dynamic_cpp` public features are ROS middleware features. + +Unit, integration, and system tests higher up in the stack, such as those found in [`test_rmw_implementation`](https://github.com/ros2/rmw_implementation/tree/master/test_rmw_implementation), [`test_rclcpp`](https://github.com/ros2/system_tests/tree/master/test_rclcpp), and [`test_communication`](https://github.com/ros2/system_tests/tree/master/test_communication) packages, provide feature coverage. [Nightly `extra_rmw_release` CI jobs](https://ci.ros2.org/view/nightly/) also test this middleware implementation. + +### Public API Testing [4.ii] + +`rmw_fastrtps_dynamic_cpp` implements the ROS middleware public API, but also provides public API of its own. + +Unit tests located in the [`test`](https://github.com/eProsima/rmw_fastrtps/tree/master/rmw_fastrtps_dynamic_cpp/test) directory provide coverage for public but `rmw_fastrtps_dynamic_cpp` specific API. +New additions or changes to this API require tests before being added. + +Unit, integration, and system tests higher up in the stack, such as those found in [`test_rmw_implementation`](https://github.com/ros2/rmw_implementation/tree/master/test_rmw_implementation), [`test_rclcpp`](https://github.com/ros2/system_tests/tree/master/test_rclcpp), and [`test_communication`](https://github.com/ros2/system_tests/tree/master/test_communication) packages, ensure compliance with the ROS middleware API specification (see [`rmw`](https://github.com/ros2/rmw) package) and further extend coverage. + +### Coverage [4.iii] + +Currently, `rmw_fastrtps_dynamic_cpp` is not included in the nightly CI coverage report. +Coverage checks should be made to consider this package to apply for a higher quality level. + +### Performance [4.iv] + +`rmw_fastrtps_dynamic_cpp` does not currently have performance tests. + +### Linters and Static Analysis [4.v] + +`rmw_fastrtps_dynamic_cpp` uses and passes all the standard linters and static analysis tools for a C++ package as described in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#linters-and-static-analysis). + +Results of the nightly linter tests can be found [here](https://ci.ros2.org/view/nightly/job/nightly_linux_extra_rmw_release/lastCompletedBuild/testReport/rmw_fastrtps_dynamic_cpp/). + +## Dependencies [5] + +### Direct Runtime ROS Dependencies [5.i]/[5.ii] + +`rmw_fastrtps_dynamic_cpp` has the following runtime ROS dependencies: +* `fastrtps_cmake_module`: [QUALITY DECLARATION](https://github.com/ros2/rosidl_typesupport_fastrtps/blob/master/fastrtps_cmake_module/QUALITY_DECLARATION.md) +* `rcpputils`: [QUALITY DECLARATION](https://github.com/ros2/rcpputils/blob/master/QUALITY_DECLARATION.md) +* `rcutils`: [QUALITY DECLARATION](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md) +* `rmw`: [QUALITY DECLARATION](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md) +* `rmw_dds_common`: [QUALITY DECLARATION](https://github.com/ros2/rmw_dds_common/blob/master/rmw_dds_common/QUALITY_DECLARATION.md) +* `rmw_fastrtps_shared_cpp`: [QUALITY DECLARATION](https://github.com/ros2/rmw_fastrtps/blob/master/rmw_fastrtps_shared_cpp/QUALITY_DECLARATION.md) +* `rosidl_runtime_c`: [QUALITY DECLARATION](https://github.com/ros2/rosidl/blob/master/rosidl_runtime_c/QUALITY_DECLARATION.md) +* `rosidl_typesupport_fastrtps_c`: [QUALITY DECLARATION](https://github.com/ros2/rosidl_typesupport_fastrtps/blob/master/rosidl_typesupport_fastrtps_c/QUALITY_DECLARATION.md) +* `rosidl_typesupport_fastrtps_cpp`: [QUALITY DECLARATION](https://github.com/ros2/rosidl_typesupport_fastrtps/blob/master/rosidl_typesupport_fastrtps_cpp/QUALITY_DECLARATION.md) +* `rosidl_typesupport_introspection_c` +* `rosidl_typesupport_introspection_cpp` + +The Quality Declarations for the last two dependencies are not available currently. + +It has several "buildtool" dependencies, which do not affect the resulting quality of the package, because they do not contribute to the public library API. +It also has several test dependencies, which do not affect the resulting quality of the package, because they are only used to build and run the test code. + +### Direct Runtime Non-ROS Dependencies [5.iii] + +`rmw_fastrtps_cpp` has the following runtime non-ROS dependencies. +* `fastcdr`: *eProsima Fast CDR* claims to be Quality Level 2. For more information, please refer to its [QUALITY DECLARATION](https://github.com/eProsima/Fast-CDR/blob/master/QUALITY.md) +* `fastrtps`: *eProsima Fast DDS* claims to be Quality Level 2. For more information, please refer to its [QUALITY DECLARATION](https://github.com/eProsima/Fast-DDS/blob/master/QUALITY.md) + +## Platform Support [6] + +`rmw_fastrtps_cpp` supports all of the tier 1 platforms as described in [REP-2000](https://www.ros.org/reps/rep-2000.html#support-tiers), and tests each change against all of them. + +Currently nightly results can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_extra_rmw_release/lastCompletedBuild/testReport/rmw_fastrtps_dynamic_cpp/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_extra_rmw_release/lastCompletedBuild/testReport/rmw_fastrtps_dynamic_cpp/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_extra_rmw_release/lastCompletedBuild/testReport/rmw_fastrtps_dynamic_cpp/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_extra_rmw_rel/lastCompletedBuild/testReport/rmw_fastrtps_dynamic_cpp/) + +## Security [7] + +### Vulnerability Disclosure Policy [7.i] + +This package conforms to the Vulnerability Disclosure Policy in [REP-2006](https://www.ros.org/reps/rep-2006.html). + +# Current status Summary + +The chart below compares the requirements in the REP-2004 with the current state of the `rmw_fastrtps_cpp` package. + +|Number| Requirement| Current state | +|--|--|--| +|1| **Version policy** |---| +|1.i|Version Policy available | ✓ | +|1.ii|Stable version |✓ | +|1.iii|Declared public API|✓| +|1.iv|API stability policy|✓| +|1.v|ABI stability policy|✓| +|1.vi_|API/ABI stable within ros distribution|✓| +|2| **Change control process** |---| +|2.i| All changes occur on change request | ✓| +|2.ii| Contributor origin (DCO, CLA, etc) | ✓| +|2.iii| Peer review policy | ✓ | +|2.iv| CI policy for change requests | ✓ | +|2.v| Documentation policy for change requests | ✓ | +|3| **Documentation** | --- | +|3.i| Per feature documentation | ✓ | +|3.ii| Per public API item documentation | * | +|3.iii| Declared License(s) | ✓ | +|3.iv| Copyright in source files| ✓ | +|3.v.a| Quality declaration linked to README | ✓ | +|3.v.b| Centralized declaration available for peer review |✓| +|4| **Testing** | --- | +|4.i| Feature items tests | ✓ | +|4.ii| Public API tests | * | +|4.iii.a| Using coverage | ☓ | +|4.iii.a| Coverage policy | ✓ | +|4.iv.a| Performance tests (if applicable) | ☓ | +|4.iv.b| Performance tests policy| ✓ | +|4.v.a| Code style enforcement (linters)| ✓ | +|4.v.b| Use of static analysis tools | ✓ | +|5| **Dependencies** | --- | +|5.i| Must not have ROS lower level dependencies | * | +|5.ii| Optional ROS lower level dependencies| ✓ | +|5.iii| Justifies quality use of non-ROS dependencies |✓| +|6| **Platform support** | --- | +|6.i| Support targets Tier1 ROS platforms| ✓ | +|7| **Security** | --- | +|7.i| Vulnerability Disclosure Policy | ✓ | diff --git a/README.md b/README.md new file mode 100644 index 0000000..164ec41 --- /dev/null +++ b/README.md @@ -0,0 +1,9 @@ +# `rmw_fastrtps_cpp` + +`rmw_fastrtps_dynamic_cpp` implements the ROS middleware interface using eProsima Fast DDS introspection typesupport at run time to decide on the serialization/deserialization mechanism. + +For more information see the repository level [README](../README.md) + +## Quality Declaration + +This package claims to be in the **Quality Level 3** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp b/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp new file mode 100644 index 0000000..67d8a72 --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp @@ -0,0 +1,42 @@ +// 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. + +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_HPP_ + +#include +#include + +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + +#include "TypeSupport.hpp" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +template +class MessageTypeSupport : public TypeSupport +{ +public: + MessageTypeSupport(const MembersType * members, const void * ros_type_support); +}; + +} // namespace rmw_fastrtps_dynamic_cpp + +#include "MessageTypeSupport_impl.hpp" + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp b/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp new file mode 100644 index 0000000..95f48e9 --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp @@ -0,0 +1,69 @@ +// 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. + +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ + +#include +#include +#include +#include + +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + +#include "rcpputils/find_and_replace.hpp" + +#include "rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp" +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +template +MessageTypeSupport::MessageTypeSupport( + const MembersType * members, const void * ros_type_support) +: TypeSupport(ros_type_support) +{ + assert(members); + this->members_ = members; + + std::ostringstream ss; + std::string message_namespace(this->members_->message_namespace_); + std::string message_name(this->members_->message_name_); + if (!message_namespace.empty()) { + // Find and replace C namespace separator with C++, in case this is using C typesupport + message_namespace = rcpputils::find_and_replace(message_namespace, "__", "::"); + ss << message_namespace << "::"; + } + ss << "dds_::" << message_name << "_"; + this->setName(ss.str().c_str()); + + // Fully bound and plain by default + this->max_size_bound_ = true; + this->is_plain_ = true; + // Encapsulation size + this->m_typeSize = 4; + if (this->members_->member_count_ != 0) { + this->m_typeSize += static_cast(this->calculateMaxSerializedSize(members, 0)); + } else { + this->m_typeSize++; + } + // Account for RTPS submessage alignment + this->m_typeSize = (this->m_typeSize + 3) & ~3; +} + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__MESSAGETYPESUPPORT_IMPL_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp b/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp new file mode 100644 index 0000000..c4ac036 --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp @@ -0,0 +1,47 @@ +// 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. + +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_HPP_ + +#include + +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + +#include "TypeSupport.hpp" +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +template +class RequestTypeSupport : public TypeSupport +{ +public: + RequestTypeSupport(const ServiceMembersType * members, const void * ros_type_support); +}; + +template +class ResponseTypeSupport : public TypeSupport +{ +public: + ResponseTypeSupport(const ServiceMembersType * members, const void * ros_type_support); +}; + +} // namespace rmw_fastrtps_dynamic_cpp + +#include "ServiceTypeSupport_impl.hpp" + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp b/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp new file mode 100644 index 0000000..45e3fc6 --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp @@ -0,0 +1,101 @@ +// 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. + +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_IMPL_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_IMPL_HPP_ + +#include +#include +#include + +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + +#include "rcpputils/find_and_replace.hpp" + +#include "rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp" +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +template +RequestTypeSupport::RequestTypeSupport( + const ServiceMembersType * members, const void * ros_type_support) +: TypeSupport(ros_type_support) +{ + assert(members); + this->members_ = members->request_members_; + + std::ostringstream ss; + std::string service_namespace(members->service_namespace_); + std::string service_name(members->service_name_); + if (!service_namespace.empty()) { + // Find and replace C namespace separator with C++, in case this is using C typesupport + service_namespace = rcpputils::find_and_replace(service_namespace, "__", "::"); + ss << service_namespace << "::"; + } + ss << "dds_::" << service_name << "_Request_"; + this->setName(ss.str().c_str()); + + // Fully bound and plain by default + this->max_size_bound_ = true; + this->is_plain_ = true; + // Encapsulation size + this->m_typeSize = 4; + if (this->members_->member_count_ != 0) { + this->m_typeSize += static_cast(this->calculateMaxSerializedSize(this->members_, 0)); + } else { + this->m_typeSize++; + } + // Account for RTPS submessage alignment + this->m_typeSize = (this->m_typeSize + 3) & ~3; +} + +template +ResponseTypeSupport::ResponseTypeSupport( + const ServiceMembersType * members, const void * ros_type_support) +: TypeSupport(ros_type_support) +{ + assert(members); + this->members_ = members->response_members_; + + std::ostringstream ss; + std::string service_namespace(members->service_namespace_); + std::string service_name(members->service_name_); + if (!service_namespace.empty()) { + // Find and replace C namespace separator with C++, in case this is using C typesupport + service_namespace = rcpputils::find_and_replace(service_namespace, "__", "::"); + ss << service_namespace << "::"; + } + ss << "dds_::" << service_name << "_Response_"; + this->setName(ss.str().c_str()); + + // Fully bound and plain by default + this->max_size_bound_ = true; + this->is_plain_ = true; + // Encapsulation size + this->m_typeSize = 4; + if (this->members_->member_count_ != 0) { + this->m_typeSize += static_cast(this->calculateMaxSerializedSize(this->members_, 0)); + } else { + this->m_typeSize++; + } + // Account for RTPS submessage alignment + this->m_typeSize = (this->m_typeSize + 3) & ~3; +} + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__SERVICETYPESUPPORT_IMPL_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp b/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp new file mode 100644 index 0000000..2708392 --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/TypeSupport.hpp @@ -0,0 +1,201 @@ +// 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. + +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_HPP_ + +#include +#include + +#include "rosidl_runtime_c/string.h" +#include "rosidl_runtime_c/string_functions.h" + +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + +#include "rcutils/logging_macros.h" + +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" +#include "rosidl_typesupport_introspection_cpp/visibility_control.h" + +#include "rosidl_typesupport_introspection_c/field_types.h" +#include "rosidl_typesupport_introspection_c/identifier.h" +#include "rosidl_typesupport_introspection_c/message_introspection.h" +#include "rosidl_typesupport_introspection_c/service_introspection.h" +#include "rosidl_typesupport_introspection_c/visibility_control.h" + +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +// Helper class that uses template specialization to read/write string types to/from a +// eprosima::fastcdr::Cdr +template +struct StringHelper; + +// For C introspection typesupport we create intermediate instances of std::string so that +// eprosima::fastcdr::Cdr can handle the string properly. +template<> +struct StringHelper +{ + using type = rosidl_runtime_c__String; + + static size_t next_field_align(void * data, size_t current_alignment) + { + auto c_string = static_cast(data); + if (!c_string) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_dynamic_cpp", + "Failed to cast data as rosidl_runtime_c__String"); + return current_alignment; + } + if (!c_string->data) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_dynamic_cpp", + "rosidl_generator_c_String had invalid data"); + return current_alignment; + } + + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + current_alignment += 4; + return current_alignment + strlen(c_string->data) + 1; + } + + static std::string convert_to_std_string(void * data) + { + auto c_string = static_cast(data); + if (!c_string) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_dynamic_cpp", + "Failed to cast data as rosidl_runtime_c__String"); + return ""; + } + if (!c_string->data) { + RCUTILS_LOG_ERROR_NAMED( + "rmw_fastrtps_dynamic_cpp", + "rosidl_generator_c_String had invalid data"); + return ""; + } + return std::string(c_string->data); + } + + static std::string convert_to_std_string(rosidl_runtime_c__String & data) + { + return std::string(data.data); + } + + static void assign(eprosima::fastcdr::Cdr & deser, void * field) + { + std::string str; + deser >> str; + rosidl_runtime_c__String * c_str = static_cast(field); + rosidl_runtime_c__String__assign(c_str, str.c_str()); + } +}; + +// For C++ introspection typesupport we just reuse the same std::string transparently. +template<> +struct StringHelper +{ + using type = std::string; + + static std::string & convert_to_std_string(void * data) + { + return *(static_cast(data)); + } + + static void assign(eprosima::fastcdr::Cdr & deser, void * field) + { + std::string & str = *(std::string *)field; + deser >> str; + } +}; + +class TypeSupportProxy : public rmw_fastrtps_shared_cpp::TypeSupport +{ +public: + explicit TypeSupportProxy(rmw_fastrtps_shared_cpp::TypeSupport * inner_type); + + size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const override; + + bool serializeROSmessage( + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const override; + + bool deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override; +}; + +class BaseTypeSupport : public rmw_fastrtps_shared_cpp::TypeSupport +{ +public: + const void * ros_type_support() const + { + return ros_type_support_; + } + +protected: + explicit BaseTypeSupport(const void * ros_type_support) + { + ros_type_support_ = ros_type_support; + } + +private: + const void * ros_type_support_; +}; + +template +class TypeSupport : public BaseTypeSupport +{ +public: + size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const override; + + bool serializeROSmessage( + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const override; + + bool deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const override; + +protected: + explicit TypeSupport(const void * ros_type_support); + + size_t calculateMaxSerializedSize(const MembersType * members, size_t current_alignment); + + const MembersType * members_; + +private: + size_t getEstimatedSerializedSize( + const MembersType * members, + const void * ros_message, + size_t current_alignment) const; + + bool serializeROSmessage( + eprosima::fastcdr::Cdr & ser, + const MembersType * members, + const void * ros_message) const; + + bool deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, + const MembersType * members, + void * ros_message) const; +}; + +} // namespace rmw_fastrtps_dynamic_cpp + +#include "TypeSupport_impl.hpp" + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp b/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp new file mode 100644 index 0000000..cfa457f --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/TypeSupport_impl.hpp @@ -0,0 +1,977 @@ +// 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. + +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_IMPL_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_IMPL_HPP_ + +#include +#include +#include + +#include "fastcdr/Cdr.h" +#include "fastcdr/FastBuffer.h" +#include "fastcdr/exceptions/Exception.h" + +#include "rmw_fastrtps_dynamic_cpp/TypeSupport.hpp" +#include "rmw_fastrtps_dynamic_cpp/macros.hpp" + +#include "rmw/error_handling.h" + +#include "rosidl_typesupport_fastrtps_c/wstring_conversion.hpp" +#include "rosidl_typesupport_fastrtps_cpp/wstring_conversion.hpp" +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" +#include "rosidl_typesupport_introspection_cpp/service_introspection.hpp" + +#include "rosidl_typesupport_introspection_c/message_introspection.h" +#include "rosidl_typesupport_introspection_c/service_introspection.h" + +#include "rosidl_runtime_c/primitives_sequence_functions.h" +#include "rosidl_runtime_c/u16string_functions.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +template +struct GenericCSequence; + +// multiple definitions of ambiguous primitive types +SPECIALIZE_GENERIC_C_SEQUENCE(bool, bool) +SPECIALIZE_GENERIC_C_SEQUENCE(byte, uint8_t) +SPECIALIZE_GENERIC_C_SEQUENCE(char, char) +SPECIALIZE_GENERIC_C_SEQUENCE(float32, float) +SPECIALIZE_GENERIC_C_SEQUENCE(float64, double) +SPECIALIZE_GENERIC_C_SEQUENCE(int8, int8_t) +SPECIALIZE_GENERIC_C_SEQUENCE(int16, int16_t) +SPECIALIZE_GENERIC_C_SEQUENCE(uint16, uint16_t) +SPECIALIZE_GENERIC_C_SEQUENCE(int32, int32_t) +SPECIALIZE_GENERIC_C_SEQUENCE(uint32, uint32_t) +SPECIALIZE_GENERIC_C_SEQUENCE(int64, int64_t) +SPECIALIZE_GENERIC_C_SEQUENCE(uint64, uint64_t) + +template +TypeSupport::TypeSupport(const void * ros_type_support) +: BaseTypeSupport(ros_type_support) +{ + m_isGetKeyDefined = false; + max_size_bound_ = false; + is_plain_ = false; +} + +// C++ specialization +template +void serialize_field( + const rosidl_typesupport_introspection_cpp::MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & ser) +{ + if (!member->is_array_) { + ser << *static_cast(field); + } else if (member->array_size_ && !member->is_upper_bound_) { + ser.serializeArray(static_cast(field), member->array_size_); + } else { + std::vector & data = *reinterpret_cast *>(field); + ser << data; + } +} + +template<> +inline +void serialize_field( + const rosidl_typesupport_introspection_cpp::MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & ser) +{ + std::wstring wstr; + if (!member->is_array_) { + auto u16str = static_cast(field); + rosidl_typesupport_fastrtps_cpp::u16string_to_wstring(*u16str, wstr); + ser << wstr; + } else { + size_t size; + if (member->array_size_ && !member->is_upper_bound_) { + size = member->array_size_; + } else { + size = member->size_function(field); + ser << static_cast(size); + } + for (size_t i = 0; i < size; ++i) { + const void * element = member->get_const_function(field, i); + auto u16str = static_cast(element); + rosidl_typesupport_fastrtps_cpp::u16string_to_wstring(*u16str, wstr); + ser << wstr; + } + } +} + +// C specialization +template +void serialize_field( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & ser) +{ + if (!member->is_array_) { + ser << *static_cast(field); + } else if (member->array_size_ && !member->is_upper_bound_) { + ser.serializeArray(static_cast(field), member->array_size_); + } else { + auto & data = *reinterpret_cast::type *>(field); + ser.serializeSequence(reinterpret_cast(data.data), data.size); + } +} + +template<> +inline +void serialize_field( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & ser) +{ + using CStringHelper = StringHelper; + if (!member->is_array_) { + auto && str = CStringHelper::convert_to_std_string(field); + // Control maximum length. + if (member->string_upper_bound_ && str.length() > member->string_upper_bound_ + 1) { + throw std::runtime_error("string overcomes the maximum length"); + } + ser << str; + } else { + // First, cast field to rosidl_generator_c + // Then convert to a std::string using StringHelper and serialize the std::string + if (member->array_size_ && !member->is_upper_bound_) { + // tmpstring is defined here and not below to avoid + // memory allocation in every iteration of the for loop + std::string tmpstring; + auto string_field = static_cast(field); + for (size_t i = 0; i < member->array_size_; ++i) { + tmpstring = string_field[i].data; + ser.serialize(tmpstring); + } + } else { + auto & string_sequence_field = + *reinterpret_cast(field); + std::vector cpp_string_vector; + for (size_t i = 0; i < string_sequence_field.size; ++i) { + cpp_string_vector.push_back( + CStringHelper::convert_to_std_string(string_sequence_field.data[i])); + } + ser << cpp_string_vector; + } + } +} + +template<> +inline +void serialize_field( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & ser) +{ + std::wstring wstr; + if (!member->is_array_) { + auto u16str = static_cast(field); + rosidl_typesupport_fastrtps_c::u16string_to_wstring(*u16str, wstr); + ser << wstr; + } else if (member->array_size_ && !member->is_upper_bound_) { + auto array = static_cast(field); + for (size_t i = 0; i < member->array_size_; ++i) { + rosidl_typesupport_fastrtps_c::u16string_to_wstring(array[i], wstr); + ser << wstr; + } + } else { + auto sequence = static_cast(field); + ser << static_cast(sequence->size); + for (size_t i = 0; i < sequence->size; ++i) { + rosidl_typesupport_fastrtps_c::u16string_to_wstring(sequence->data[i], wstr); + ser << wstr; + } + } +} + +template +bool TypeSupport::serializeROSmessage( + eprosima::fastcdr::Cdr & ser, + const MembersType * members, + const void * ros_message) const +{ + assert(members); + assert(ros_message); + + for (uint32_t i = 0; i < members->member_count_; ++i) { + const auto member = members->members_ + i; + void * field = const_cast(static_cast(ros_message)) + member->offset_; + switch (member->type_id_) { + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: + if (!member->is_array_) { + // don't cast to bool here because if the bool is + // uninitialized the random value can't be deserialized + ser << (*static_cast(field) ? true : false); + } else { + serialize_field(member, field, ser); + } + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + serialize_field(member, field, ser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + { + auto sub_members = static_cast(member->members_->data); + if (!member->is_array_) { + serializeROSmessage(ser, sub_members, field); + } else { + size_t array_size = 0; + + if (member->array_size_ && !member->is_upper_bound_) { + array_size = member->array_size_; + } else { + if (!member->size_function) { + RMW_SET_ERROR_MSG("unexpected error: size function is null"); + return false; + } + array_size = member->size_function(field); + + // Serialize length + ser << (uint32_t)array_size; + } + + if (array_size != 0 && !member->get_function) { + RMW_SET_ERROR_MSG("unexpected error: get_function function is null"); + return false; + } + for (size_t index = 0; index < array_size; ++index) { + serializeROSmessage(ser, sub_members, member->get_function(field, index)); + } + } + } + break; + default: + throw std::runtime_error("unknown type"); + } + } + + return true; +} + +// C++ specialization +template +size_t next_field_align( + const rosidl_typesupport_introspection_cpp::MessageMember * member, + void * field, + size_t current_alignment) +{ + const size_t padding = 4; + size_t item_size = sizeof(T); + if (!member->is_array_) { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); + current_alignment += item_size; + } else if (member->array_size_ && !member->is_upper_bound_) { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); + current_alignment += item_size * member->array_size_; + } else { + std::vector & data = *reinterpret_cast *>(field); + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + if (!data.empty()) { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); + current_alignment += item_size * data.size(); + } + } + return current_alignment; +} + +template +size_t next_field_align_string( + const rosidl_typesupport_introspection_cpp::MessageMember * member, + void * field, + size_t current_alignment) +{ + const size_t padding = 4; + size_t character_size = + (member->type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING) ? 4 : 1; + if (!member->is_array_) { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + auto & str = *static_cast(field); + current_alignment += character_size * (str.size() + 1); + } else if (member->array_size_ && !member->is_upper_bound_) { + auto str_arr = static_cast(field); + for (size_t index = 0; index < member->array_size_; ++index) { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + current_alignment += character_size * (str_arr[index].size() + 1); + } + } else { + auto & data = *reinterpret_cast *>(field); + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + for (auto & it : data) { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + current_alignment += character_size * (it.size() + 1); + } + } + return current_alignment; +} + +// C specialization +template +size_t next_field_align( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + size_t current_alignment) +{ + const size_t padding = 4; + size_t item_size = sizeof(T); + if (!member->is_array_) { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); + current_alignment += item_size; + } else if (member->array_size_ && !member->is_upper_bound_) { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); + current_alignment += item_size * member->array_size_; + } else { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + + auto & data = *reinterpret_cast::type *>(field); + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, item_size); + current_alignment += item_size * data.size; + } + return current_alignment; +} + +template +size_t next_field_align_string( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + size_t current_alignment); + +template<> +inline +size_t next_field_align_string( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + size_t current_alignment) +{ + const size_t padding = 4; + using CStringHelper = StringHelper; + if (!member->is_array_) { + current_alignment = CStringHelper::next_field_align(field, current_alignment); + } else { + if (member->array_size_ && !member->is_upper_bound_) { + auto string_field = static_cast(field); + for (size_t i = 0; i < member->array_size_; ++i) { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + current_alignment += strlen(string_field[i].data) + 1; + } + } else { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + auto & string_sequence_field = + *reinterpret_cast(field); + for (size_t i = 0; i < string_sequence_field.size; ++i) { + current_alignment = CStringHelper::next_field_align( + &(string_sequence_field.data[i]), current_alignment); + } + } + } + return current_alignment; +} + +template<> +inline +size_t next_field_align_string( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + size_t current_alignment) +{ + const size_t padding = 4; + if (!member->is_array_) { + auto u16str = static_cast(field); + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + current_alignment += 4 * (u16str->size + 1); + } else { + if (member->array_size_ && !member->is_upper_bound_) { + auto string_field = static_cast(field); + for (size_t i = 0; i < member->array_size_; ++i) { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + current_alignment += 4 * (string_field[i].size + 1); + } + } else { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + auto & string_sequence_field = + *reinterpret_cast(field); + for (size_t i = 0; i < string_sequence_field.size; ++i) { + current_alignment += eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + current_alignment += padding; + current_alignment += 4 * (string_sequence_field.data[i].size + 1); + } + } + } + return current_alignment; +} + +template +size_t TypeSupport::getEstimatedSerializedSize( + const MembersType * members, + const void * ros_message, + size_t current_alignment) const +{ + assert(members); + assert(ros_message); + + size_t initial_alignment = current_alignment; + + for (uint32_t i = 0; i < members->member_count_; ++i) { + const auto member = members->members_ + i; + void * field = const_cast(static_cast(ros_message)) + member->offset_; + switch (member->type_id_) { + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: + current_alignment = next_field_align(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + current_alignment = next_field_align(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + current_alignment = next_field_align(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: + current_alignment = next_field_align(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: + current_alignment = next_field_align(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + current_alignment = next_field_align(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + current_alignment = next_field_align(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + current_alignment = next_field_align(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + current_alignment = next_field_align(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + current_alignment = next_field_align(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + current_alignment = next_field_align(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + current_alignment = next_field_align_string(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + current_alignment = next_field_align_string(member, field, current_alignment); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + { + auto sub_members = static_cast(member->members_->data); + if (!member->is_array_) { + current_alignment += getEstimatedSerializedSize(sub_members, field, current_alignment); + } else { + size_t array_size = 0; + + if (member->array_size_ && !member->is_upper_bound_) { + array_size = member->array_size_; + } else { + if (!member->size_function) { + RMW_SET_ERROR_MSG("unexpected error: size function is null"); + return false; + } + array_size = member->size_function(field); + + // Length serialization + current_alignment += 4 + eprosima::fastcdr::Cdr::alignment(current_alignment, 4); + } + + if (array_size != 0 && !member->get_function) { + RMW_SET_ERROR_MSG("unexpected error: get_function function is null"); + return false; + } + for (size_t index = 0; index < array_size; ++index) { + current_alignment += getEstimatedSerializedSize( + sub_members, + member->get_function(field, index), + current_alignment); + } + } + } + break; + default: + throw std::runtime_error("unknown type"); + } + } + + return current_alignment - initial_alignment; +} + +template +void deserialize_field( + const rosidl_typesupport_introspection_cpp::MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & deser) +{ + if (!member->is_array_) { + deser >> *static_cast(field); + } else if (member->array_size_ && !member->is_upper_bound_) { + deser.deserializeArray(static_cast(field), member->array_size_); + } else { + auto & vector = *reinterpret_cast *>(field); + deser >> vector; + } +} + +template<> +inline void deserialize_field( + const rosidl_typesupport_introspection_cpp::MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & deser) +{ + if (!member->is_array_) { + deser >> *static_cast(field); + } else if (member->array_size_ && !member->is_upper_bound_) { + std::string * array = static_cast(field); + deser.deserializeArray(array, member->array_size_); + } else { + auto & vector = *reinterpret_cast *>(field); + deser >> vector; + } +} + +template<> +inline void deserialize_field( + const rosidl_typesupport_introspection_cpp::MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & deser) +{ + std::wstring wstr; + if (!member->is_array_) { + deser >> wstr; + rosidl_typesupport_fastrtps_cpp::wstring_to_u16string( + wstr, *static_cast(field)); + } else { + uint32_t size; + if (member->array_size_ && !member->is_upper_bound_) { + size = static_cast(member->array_size_); + } else { + deser >> size; + member->resize_function(field, size); + } + for (size_t i = 0; i < size; ++i) { + void * element = member->get_function(field, i); + auto u16str = static_cast(element); + deser >> wstr; + rosidl_typesupport_fastrtps_cpp::wstring_to_u16string(wstr, *u16str); + } + } +} + +template +void deserialize_field( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & deser) +{ + if (!member->is_array_) { + deser >> *static_cast(field); + } else if (member->array_size_ && !member->is_upper_bound_) { + deser.deserializeArray(static_cast(field), member->array_size_); + } else { + auto & data = *reinterpret_cast::type *>(field); + int32_t dsize = 0; + deser >> dsize; + GenericCSequence::init(&data, dsize); + deser.deserializeArray(reinterpret_cast(data.data), dsize); + } +} + +template<> +inline void deserialize_field( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & deser) +{ + if (!member->is_array_) { + using CStringHelper = StringHelper; + CStringHelper::assign(deser, field); + } else { + if (member->array_size_ && !member->is_upper_bound_) { + auto deser_field = static_cast(field); + // tmpstring is defined here and not below to avoid + // memory allocation in every iteration of the for loop + std::string tmpstring; + for (size_t i = 0; i < member->array_size_; ++i) { + deser.deserialize(tmpstring); + if (!rosidl_runtime_c__String__assign(&deser_field[i], tmpstring.c_str())) { + throw std::runtime_error("unable to assign rosidl_runtime_c__String"); + } + } + } else { + std::vector cpp_string_vector; + deser >> cpp_string_vector; + + auto & string_sequence_field = + *reinterpret_cast(field); + if ( + !rosidl_runtime_c__String__Sequence__init( + &string_sequence_field, cpp_string_vector.size())) + { + throw std::runtime_error("unable to initialize rosidl_runtime_c__String array"); + } + + for (size_t i = 0; i < cpp_string_vector.size(); ++i) { + if ( + !rosidl_runtime_c__String__assign( + &string_sequence_field.data[i], cpp_string_vector[i].c_str())) + { + throw std::runtime_error("unable to assign rosidl_runtime_c__String"); + } + } + } + } +} + +template<> +inline void deserialize_field( + const rosidl_typesupport_introspection_c__MessageMember * member, + void * field, + eprosima::fastcdr::Cdr & deser) +{ + std::wstring wstr; + if (!member->is_array_) { + deser >> wstr; + rosidl_typesupport_fastrtps_c::wstring_to_u16string( + wstr, *static_cast(field)); + } else if (member->array_size_ && !member->is_upper_bound_) { + auto array = static_cast(field); + for (size_t i = 0; i < member->array_size_; ++i) { + deser >> wstr; + rosidl_typesupport_fastrtps_c::wstring_to_u16string(wstr, array[i]); + } + } else { + uint32_t size; + deser >> size; + auto sequence = static_cast(field); + if (!rosidl_runtime_c__U16String__Sequence__init(sequence, size)) { + throw std::runtime_error("unable to initialize rosidl_runtime_c__U16String sequence"); + } + for (size_t i = 0; i < sequence->size; ++i) { + deser >> wstr; + rosidl_typesupport_fastrtps_c::wstring_to_u16string(wstr, sequence->data[i]); + } + } +} + +template +bool TypeSupport::deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, + const MembersType * members, + void * ros_message) const +{ + assert(members); + assert(ros_message); + + for (uint32_t i = 0; i < members->member_count_; ++i) { + const auto * member = members->members_ + i; + void * field = static_cast(ros_message) + member->offset_; + switch (member->type_id_) { + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: + deserialize_field(member, field, deser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + deserialize_field(member, field, deser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + deserialize_field(member, field, deser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: + deserialize_field(member, field, deser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: + deserialize_field(member, field, deser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + deserialize_field(member, field, deser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + deserialize_field(member, field, deser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + deserialize_field(member, field, deser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + deserialize_field(member, field, deser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + deserialize_field(member, field, deser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + deserialize_field(member, field, deser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + deserialize_field(member, field, deser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + deserialize_field(member, field, deser); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + { + auto sub_members = static_cast(member->members_->data); + if (!member->is_array_) { + if (!deserializeROSmessage(deser, sub_members, field)) { + return false; + } + } else { + size_t array_size = 0; + + if (member->array_size_ && !member->is_upper_bound_) { + array_size = member->array_size_; + } else { + uint32_t num_elems = 0; + deser >> num_elems; + array_size = static_cast(num_elems); + + if (!member->resize_function) { + RMW_SET_ERROR_MSG("unexpected error: resize function is null"); + return false; + } + member->resize_function(field, array_size); + } + + if (array_size != 0 && !member->get_function) { + RMW_SET_ERROR_MSG("unexpected error: get_function function is null"); + return false; + } + for (size_t index = 0; index < array_size; ++index) { + if (!deserializeROSmessage(deser, sub_members, member->get_function(field, index))) { + return false; + } + } + } + } + break; + default: + throw std::runtime_error("unknown type"); + } + } + + return true; +} + +template +size_t TypeSupport::calculateMaxSerializedSize( + const MembersType * members, size_t current_alignment) +{ + assert(members); + + size_t initial_alignment = current_alignment; + + const size_t padding = 4; + + for (uint32_t i = 0; i < members->member_count_; ++i) { + const auto * member = members->members_ + i; + + size_t array_size = 1; + if (member->is_array_) { + array_size = member->array_size_; + + // Whether it is unbounded. + if (0u == array_size) { + this->max_size_bound_ = false; + } + + // Whether it is a sequence. + if (0 == array_size || member->is_upper_bound_) { + this->is_plain_ = false; + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding); + } + } + + switch (member->type_id_) { + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOL: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BYTE: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + current_alignment += array_size * sizeof(int8_t); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + current_alignment += array_size * sizeof(uint16_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint16_t)); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT32: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + current_alignment += array_size * sizeof(uint32_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint32_t)); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT64: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + current_alignment += array_size * sizeof(uint64_t) + + eprosima::fastcdr::Cdr::alignment(current_alignment, sizeof(uint64_t)); + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + { + this->max_size_bound_ = false; + this->is_plain_ = false; + size_t character_size = + (member->type_id_ == rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING) ? 4 : 1; + for (size_t index = 0; index < array_size; ++index) { + current_alignment += padding + + eprosima::fastcdr::Cdr::alignment(current_alignment, padding) + + character_size * (member->string_upper_bound_ + 1); + } + } + break; + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + { + auto sub_members = static_cast(member->members_->data); + for (size_t index = 0; index < array_size; ++index) { + current_alignment += calculateMaxSerializedSize(sub_members, current_alignment); + } + } + break; + default: + throw std::runtime_error("unknown type"); + } + } + + return current_alignment - initial_alignment; +} + +template +size_t TypeSupport::getEstimatedSerializedSize( + const void * ros_message, const void * impl) const +{ + if (is_plain_) { + return m_typeSize; + } + + assert(ros_message); + assert(members_); + + // Encapsulation size + size_t ret_val = 4; + + (void)impl; + if (members_->member_count_ != 0) { + ret_val += TypeSupport::getEstimatedSerializedSize(members_, ros_message, 0); + } else { + ret_val += 1; + } + + return ret_val; +} + +template +bool TypeSupport::serializeROSmessage( + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const +{ + assert(ros_message); + assert(members_); + + // Serialize encapsulation + ser.serialize_encapsulation(); + + (void)impl; + if (members_->member_count_ != 0) { + TypeSupport::serializeROSmessage(ser, members_, ros_message); + } else { + ser << (uint8_t)0; + } + + return true; +} + +template +bool TypeSupport::deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const +{ + assert(ros_message); + assert(members_); + + try { + // Deserialize encapsulation. + deser.read_encapsulation(); + + (void)impl; + if (members_->member_count_ != 0) { + return TypeSupport::deserializeROSmessage(deser, members_, ros_message); + } + + uint8_t dump = 0; + deser >> dump; + (void)dump; + } catch (const eprosima::fastcdr::exception::Exception &) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Fast CDR exception deserializing message of type %s.", + getName()); + return false; + } + + return true; +} + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__TYPESUPPORT_IMPL_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/get_client.hpp b/include/rmw_fastrtps_dynamic_cpp/get_client.hpp new file mode 100644 index 0000000..b4357de --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/get_client.hpp @@ -0,0 +1,51 @@ +// Copyright 2017 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__GET_CLIENT_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__GET_CLIENT_HPP_ + +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" + +#include "rmw/rmw.h" +#include "rmw_fastrtps_dynamic_cpp/visibility_control.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +/// Return a native Fast DDS DataWriter handle for the request. +/** + * The function returns `NULL` when either the client handle is `NULL` or + * when the client handle is from a different rmw implementation. + * + * \return native Fast DDS DataWriter handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastdds::dds::DataWriter * +get_request_datawriter(rmw_client_t * client); + +/// Return a native Fast DDS DataReader handle for the response. +/** + * The function returns `NULL` when either the client handle is `NULL` or + * when the client handle is from a different rmw implementation. + * + * \return native Fast DDS DataReader handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastdds::dds::DataReader * +get_response_datareader(rmw_client_t * client); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__GET_CLIENT_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp b/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp new file mode 100644 index 0000000..53c2289 --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/get_participant.hpp @@ -0,0 +1,39 @@ +// Copyright 2017 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__GET_PARTICIPANT_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__GET_PARTICIPANT_HPP_ + +#include "fastdds/dds/domain/DomainParticipant.hpp" + +#include "rmw/rmw.h" +#include "rmw_fastrtps_dynamic_cpp/visibility_control.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +/// Return a native Fast DDS DomainParticipant handle. +/** + * The function returns `NULL` when either the node handle is `NULL` or when the + * node handle is from a different rmw implementation. + * + * \return native Fast DDS DomainParticipant handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastdds::dds::DomainParticipant * +get_domain_participant(rmw_node_t * node); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__GET_PARTICIPANT_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp b/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp new file mode 100644 index 0000000..28817bc --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/get_publisher.hpp @@ -0,0 +1,39 @@ +// Copyright 2017 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__GET_PUBLISHER_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__GET_PUBLISHER_HPP_ + +#include "fastdds/dds/publisher/DataWriter.hpp" + +#include "rmw/rmw.h" +#include "rmw_fastrtps_dynamic_cpp/visibility_control.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +/// Return a native Fast DDS DataWriter handle. +/** + * The function returns `NULL` when either the publisher handle is `NULL` or + * when the publisher handle is from a different rmw implementation. + * + * \return native Fast DDS DataWriter handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastdds::dds::DataWriter * +get_datawriter(rmw_publisher_t * publisher); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__GET_PUBLISHER_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/get_service.hpp b/include/rmw_fastrtps_dynamic_cpp/get_service.hpp new file mode 100644 index 0000000..753a74b --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/get_service.hpp @@ -0,0 +1,51 @@ +// Copyright 2017 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__GET_SERVICE_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__GET_SERVICE_HPP_ + +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" + +#include "rmw/rmw.h" +#include "rmw_fastrtps_dynamic_cpp/visibility_control.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +/// Return a native Fast DDS DataReader handle for the request. +/** + * The function returns `NULL` when either the service handle is `NULL` or + * when the service handle is from a different rmw implementation. + * + * \return native Fast DDS DataReader handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastdds::dds::DataReader * +get_request_datareader(rmw_service_t * service); + +/// Return a native Fast DDS DataWriter handle for the response. +/** + * The function returns `NULL` when either the service handle is `NULL` or + * when the service handle is from a different rmw implementation. + * + * \return native Fast DDS DataWriter handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastdds::dds::DataWriter * +get_response_datawriter(rmw_service_t * service); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__GET_SERVICE_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp b/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp new file mode 100644 index 0000000..a61b2bc --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/get_subscriber.hpp @@ -0,0 +1,39 @@ +// Copyright 2017 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__GET_SUBSCRIBER_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__GET_SUBSCRIBER_HPP_ + +#include "fastdds/dds/subscriber/DataReader.hpp" + +#include "rmw/rmw.h" +#include "rmw_fastrtps_dynamic_cpp/visibility_control.h" + +namespace rmw_fastrtps_dynamic_cpp +{ + +/// Return a native Fast DDS DataReader handle. +/** + * The function returns `NULL` when either the subscription handle is `NULL` or + * when the subscription handle is from a different rmw implementation. + * + * \return native Fast DDS DataReader handle if successful, otherwise `NULL` + */ +RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC +eprosima::fastdds::dds::DataReader * +get_datareader(rmw_subscription_t * subscription); + +} // namespace rmw_fastrtps_dynamic_cpp + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__GET_SUBSCRIBER_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/identifier.hpp b/include/rmw_fastrtps_dynamic_cpp/identifier.hpp new file mode 100644 index 0000000..3983ff9 --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/identifier.hpp @@ -0,0 +1,20 @@ +// 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. + +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__IDENTIFIER_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__IDENTIFIER_HPP_ + +extern const char * const eprosima_fastrtps_identifier; + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__IDENTIFIER_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp b/include/rmw_fastrtps_dynamic_cpp/init_rmw_context_impl.hpp new file mode 100644 index 0000000..11cc71d --- /dev/null +++ b/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/include/rmw_fastrtps_dynamic_cpp/macros.hpp b/include/rmw_fastrtps_dynamic_cpp/macros.hpp new file mode 100644 index 0000000..19d66de --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/macros.hpp @@ -0,0 +1,36 @@ +// Copyright 2016 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__MACROS_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__MACROS_HPP_ + +#include +#include + +#define SPECIALIZE_GENERIC_C_SEQUENCE(C_NAME, C_TYPE) \ + template<> \ + struct GenericCSequence \ + { \ + using type = rosidl_runtime_c__ ## C_NAME ## __Sequence; \ + \ + static void fini(type * array) { \ + rosidl_runtime_c__ ## C_NAME ## __Sequence__fini(array); \ + } \ + \ + static bool init(type * array, size_t size) { \ + return rosidl_runtime_c__ ## C_NAME ## __Sequence__init(array, size); \ + } \ + }; + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__MACROS_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/publisher.hpp b/include/rmw_fastrtps_dynamic_cpp/publisher.hpp new file mode 100644 index 0000000..4f98b6c --- /dev/null +++ b/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/include/rmw_fastrtps_dynamic_cpp/serialization_format.hpp b/include/rmw_fastrtps_dynamic_cpp/serialization_format.hpp new file mode 100644 index 0000000..de429b8 --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/serialization_format.hpp @@ -0,0 +1,20 @@ +// Copyright 2018 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__SERIALIZATION_FORMAT_HPP_ +#define RMW_FASTRTPS_DYNAMIC_CPP__SERIALIZATION_FORMAT_HPP_ + +extern const char * const eprosima_fastrtps_dynamic_serialization_format; + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__SERIALIZATION_FORMAT_HPP_ diff --git a/include/rmw_fastrtps_dynamic_cpp/subscription.hpp b/include/rmw_fastrtps_dynamic_cpp/subscription.hpp new file mode 100644 index 0000000..0db8790 --- /dev/null +++ b/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/include/rmw_fastrtps_dynamic_cpp/visibility_control.h b/include/rmw_fastrtps_dynamic_cpp/visibility_control.h new file mode 100644 index 0000000..39a0eb6 --- /dev/null +++ b/include/rmw_fastrtps_dynamic_cpp/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2017 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef RMW_FASTRTPS_DYNAMIC_CPP__VISIBILITY_CONTROL_H_ +#define RMW_FASTRTPS_DYNAMIC_CPP__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define RMW_FASTRTPS_DYNAMIC_CPP_EXPORT __attribute__ ((dllexport)) + #define RMW_FASTRTPS_DYNAMIC_CPP_IMPORT __attribute__ ((dllimport)) + #else + #define RMW_FASTRTPS_DYNAMIC_CPP_EXPORT __declspec(dllexport) + #define RMW_FASTRTPS_DYNAMIC_CPP_IMPORT __declspec(dllimport) + #endif + #ifdef RMW_FASTRTPS_DYNAMIC_CPP_BUILDING_LIBRARY + #define RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC RMW_FASTRTPS_DYNAMIC_CPP_EXPORT + #else + #define RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC RMW_FASTRTPS_DYNAMIC_CPP_IMPORT + #endif + #define RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC_TYPE RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC + #define RMW_FASTRTPS_DYNAMIC_CPP_LOCAL +#else + #define RMW_FASTRTPS_DYNAMIC_CPP_EXPORT __attribute__ ((visibility("default"))) + #define RMW_FASTRTPS_DYNAMIC_CPP_IMPORT + #if __GNUC__ >= 4 + #define RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC __attribute__ ((visibility("default"))) + #define RMW_FASTRTPS_DYNAMIC_CPP_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC + #define RMW_FASTRTPS_DYNAMIC_CPP_LOCAL + #endif + #define RMW_FASTRTPS_DYNAMIC_CPP_PUBLIC_TYPE +#endif + +#endif // RMW_FASTRTPS_DYNAMIC_CPP__VISIBILITY_CONTROL_H_ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..59f5bfc --- /dev/null +++ b/package.xml @@ -0,0 +1,57 @@ + + + + rmw_fastrtps_dynamic_cpp + 6.2.1 + Implement the ROS middleware interface using introspection type support. + Michel Hidalgo + Michael Jeronimo + Apache License 2.0 + Ricardo González + Dirk Thomas + + ament_cmake_ros + fastrtps_cmake_module + + ament_cmake + + fastcdr + fastrtps + fastrtps_cmake_module + rcpputils + rcutils + rmw + rmw_dds_common + rmw_fastrtps_shared_cpp + rosidl_runtime_c + rosidl_typesupport_fastrtps_c + rosidl_typesupport_fastrtps_cpp + rosidl_typesupport_introspection_c + rosidl_typesupport_introspection_cpp + + fastcdr + fastrtps + fastrtps_cmake_module + rcpputils + rcutils + rmw + rmw_dds_common + rmw_fastrtps_shared_cpp + rosidl_runtime_c + rosidl_typesupport_fastrtps_c + rosidl_typesupport_fastrtps_cpp + rosidl_typesupport_introspection_c + rosidl_typesupport_introspection_cpp + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + osrf_testing_tools_cpp + test_msgs + + rmw_implementation_packages + + + ament_cmake + + diff --git a/rmw_fastrtps_dynamic_cpp-extras.cmake b/rmw_fastrtps_dynamic_cpp-extras.cmake new file mode 100644 index 0000000..2db8a7b --- /dev/null +++ b/rmw_fastrtps_dynamic_cpp-extras.cmake @@ -0,0 +1,24 @@ +# Copyright 2017 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. + +# copied from rmw_fastrtps_dynamic_cpp/rmw_fastrtps_dynamic_cpp-extras.cmake + +find_package(fastrtps_cmake_module REQUIRED) +find_package(fastcdr REQUIRED CONFIG) +find_package(fastrtps REQUIRED CONFIG) +find_package(FastRTPS REQUIRED MODULE) + +list(APPEND rmw_fastrtps_dynamic_cpp_INCLUDE_DIRS ${FastRTPS_INCLUDE_DIR}) +# specific order: dependents before dependencies +list(APPEND rmw_fastrtps_dynamic_cpp_LIBRARIES fastrtps fastcdr) diff --git a/src/client_service_common.cpp b/src/client_service_common.cpp new file mode 100644 index 0000000..7677079 --- /dev/null +++ b/src/client_service_common.cpp @@ -0,0 +1,46 @@ +// 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 "client_service_common.hpp" +#include "type_support_common.hpp" + +const void * +get_request_ptr(const void * untyped_service_members, const char * typesupport) +{ + if (using_introspection_c_typesupport(typesupport)) { + return get_request_ptr( + untyped_service_members); + } else if (using_introspection_cpp_typesupport(typesupport)) { + return get_request_ptr( + untyped_service_members); + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return nullptr; +} + +const void * +get_response_ptr(const void * untyped_service_members, const char * typesupport) +{ + if (using_introspection_c_typesupport(typesupport)) { + return get_response_ptr( + untyped_service_members); + } else if (using_introspection_cpp_typesupport(typesupport)) { + return get_response_ptr( + untyped_service_members); + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return nullptr; +} diff --git a/src/client_service_common.hpp b/src/client_service_common.hpp new file mode 100644 index 0000000..2020ff5 --- /dev/null +++ b/src/client_service_common.hpp @@ -0,0 +1,46 @@ +// 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. + +#ifndef CLIENT_SERVICE_COMMON_HPP_ +#define CLIENT_SERVICE_COMMON_HPP_ + +#include "rmw/error_handling.h" + +template +const void * get_request_ptr(const void * untyped_service_members) +{ + auto service_members = static_cast(untyped_service_members); + if (!service_members) { + RMW_SET_ERROR_MSG("service members handle is null"); + return nullptr; + } + return service_members->request_members_; +} + +const void * get_request_ptr(const void * untyped_service_members, const char * typesupport); + +template +const void * get_response_ptr(const void * untyped_service_members) +{ + auto service_members = static_cast(untyped_service_members); + if (!service_members) { + RMW_SET_ERROR_MSG("service members handle is null"); + return nullptr; + } + return service_members->response_members_; +} + +const void * get_response_ptr(const void * untyped_service_members, const char * typesupport); + +#endif // CLIENT_SERVICE_COMMON_HPP_ diff --git a/src/get_client.cpp b/src/get_client.cpp new file mode 100644 index 0000000..69a7af4 --- /dev/null +++ b/src/get_client.cpp @@ -0,0 +1,49 @@ +// Copyright 2017 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/get_client.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +eprosima::fastdds::dds::DataWriter * +get_request_datawriter(rmw_client_t * client) +{ + if (!client) { + return nullptr; + } + if (client->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(client->data); + return impl->request_writer_; +} + +eprosima::fastdds::dds::DataReader * +get_response_datareader(rmw_client_t * client) +{ + if (!client) { + return nullptr; + } + if (client->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(client->data); + return impl->response_reader_; +} + +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/src/get_participant.cpp b/src/get_participant.cpp new file mode 100644 index 0000000..23a2c56 --- /dev/null +++ b/src/get_participant.cpp @@ -0,0 +1,39 @@ +// Copyright 2017 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/get_participant.hpp" + +#include "fastdds/dds/domain/DomainParticipant.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 +{ + +eprosima::fastdds::dds::DomainParticipant * +get_domain_participant(rmw_node_t * node) +{ + if (!node) { + return nullptr; + } + if (node->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(node->context->impl->participant_info); + return impl->participant_; +} + +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/src/get_publisher.cpp b/src/get_publisher.cpp new file mode 100644 index 0000000..2b5b5da --- /dev/null +++ b/src/get_publisher.cpp @@ -0,0 +1,36 @@ +// Copyright 2017 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/get_publisher.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +eprosima::fastdds::dds::DataWriter * +get_datawriter(rmw_publisher_t * publisher) +{ + if (!publisher) { + return nullptr; + } + if (publisher->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(publisher->data); + return impl->data_writer_; +} + +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/src/get_service.cpp b/src/get_service.cpp new file mode 100644 index 0000000..b93ea26 --- /dev/null +++ b/src/get_service.cpp @@ -0,0 +1,49 @@ +// Copyright 2017 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/get_service.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +eprosima::fastdds::dds::DataReader * +get_request_datareader(rmw_service_t * service) +{ + if (!service) { + return nullptr; + } + if (service->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(service->data); + return impl->request_reader_; +} + +eprosima::fastdds::dds::DataWriter * +get_response_datawriter(rmw_service_t * service) +{ + if (!service) { + return nullptr; + } + if (service->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(service->data); + return impl->response_writer_; +} + +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/src/get_subscriber.cpp b/src/get_subscriber.cpp new file mode 100644 index 0000000..c3c51fa --- /dev/null +++ b/src/get_subscriber.cpp @@ -0,0 +1,36 @@ +// Copyright 2017 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/get_subscriber.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +eprosima::fastdds::dds::DataReader * +get_datareader(rmw_subscription_t * subscription) +{ + if (!subscription) { + return nullptr; + } + if (subscription->implementation_identifier != eprosima_fastrtps_identifier) { + return nullptr; + } + auto impl = static_cast(subscription->data); + return impl->data_reader_; +} + +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/src/identifier.cpp b/src/identifier.cpp new file mode 100644 index 0000000..1c47288 --- /dev/null +++ b/src/identifier.cpp @@ -0,0 +1,17 @@ +// 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_dynamic_cpp/identifier.hpp" + +const char * const eprosima_fastrtps_identifier = "rmw_fastrtps_dynamic_cpp"; diff --git a/src/init_rmw_context_impl.cpp b/src/init_rmw_context_impl.cpp new file mode 100644 index 0000000..aaa658f --- /dev/null +++ b/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->actual_domain_id, + &context->options.security_options, + (context->options.localhost_only == RMW_LOCALHOST_ONLY_ENABLED) ? 1 : 0, + context->options.enclave, + 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->gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, participant_info->participant_->guid()); + 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.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->graph_cache.add_participant( + common_context->gid, + context->options.enclave); + + 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/src/publisher.cpp b/src/publisher.cpp new file mode 100644 index 0000000..6235cf7 --- /dev/null +++ b/src/publisher.cpp @@ -0,0 +1,332 @@ +// 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 "fastdds/dds/core/policy/QosPolicies.hpp" +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" + +#include "fastdds/rtps/resources/ResourceManagement.h" + +#include "rcutils/error_handling.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/validate_full_topic_name.h" + +#include "rcpputils/scope_exit.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_shared_cpp/utils.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 DataSharingKind = eprosima::fastdds::dds::DataSharingKind; +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) +{ + ///// + // Check input parameters + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); + + RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); + if (0 == strlen(topic_name)) { + RMW_SET_ERROR_MSG("create_publisher() called with an empty topic_name argument"); + return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + if (!qos_policies->avoid_ros_namespace_conventions) { + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; + } + if (RMW_TOPIC_VALID != validation_result) { + const char * reason = rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_publisher() called with invalid topic name: %s", reason); + return nullptr; + } + } + RMW_CHECK_ARGUMENT_FOR_NULL(publisher_options, nullptr); + + if (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED == + publisher_options->require_unique_network_flow_endpoints) + { + RMW_SET_ERROR_MSG("Unique network flow endpoints not supported on publishers"); + return nullptr; + } + + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_publisher() called with invalid QoS"); + return nullptr; + } + + ///// + // Get RMW Type Support + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; + } + } + + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topic and type + + // Create Topic and Type names + std::string type_name = _create_type_name( + type_support->data, type_support->typesupport_identifier); + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + eprosima::fastdds::dds::TypeSupport fastdds_type; + eprosima::fastdds::dds::TopicDescription * des_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + topic_name_mangled, + type_name, + &des_topic, + &fastdds_type)) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_publisher() called with existing topic name %s with incompatible type %s", + topic_name_mangled.c_str(), type_name.c_str()); + return nullptr; + } + + ///// + // Get Participant and Publisher + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; + + ///// + // Create the custom Publisher struct (info) + auto info = new (std::nothrow) CustomPublisherInfo(); + if (!info) { + RMW_SET_ERROR_MSG("create_publisher() failed to allocate CustomPublisherInfo"); + return nullptr; + } + + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() { + delete info->listener_; + if (info->type_support_) { + dds_participant->unregister_type(info->type_support_.get_type_name()); + } + delete info; + }); + + ///// + // Create the Type Support struct + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto type_impl = type_registry.get_message_type_support(type_support); + if (!type_impl) { + RMW_SET_ERROR_MSG("create_publisher() failed to get message_type_support"); + return nullptr; + } + auto return_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { + type_registry.return_message_type_support(type_support); + }); + + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_impl; + + if (!fastdds_type) { + auto tsupport = new (std::nothrow) TypeSupportProxy(type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_publisher() failed to allocate TypeSupportProxy"); + return nullptr; + } + + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); + } + + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_publisher() requested a keyed topic with a non-keyed type"); + return nullptr; + } + + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { + RMW_SET_ERROR_MSG("create_publisher() failed to register type"); + return nullptr; + } + + info->type_support_ = fastdds_type; + + ///// + // Create Listener + 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"); + return nullptr; + } + } + + ///// + // Create and register Topic + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + + if (!get_topic_qos(*qos_policies, topic_qos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); + return nullptr; + } + + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, des_topic, + topic_name_mangled, type_name, topic_qos, true, &topic)) + { + RMW_SET_ERROR_MSG("create_publisher() failed to create topic"); + return nullptr; + } + + ///// + // Create DataWriter + + // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load + // datawriter which profile name matches with topic_name. If such profile does not exist, + // then use the default Fast DDS QoS. + eprosima::fastdds::dds::DataWriterQos writer_qos = publisher->get_default_datawriter_qos(); + + // Try to load the profile with the topic name + // It does not need to check the return code, as if the profile does not exist, + // the QoS is already the default + publisher->get_datawriter_qos_from_profile(topic_name, writer_qos); + + // Modify specific DataWriter Qos + if (!participant_info->leave_middleware_default_qos) { + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { + writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } + + writer_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + writer_qos.data_sharing().off(); + } + + // Get QoS from RMW + if (!get_datawriter_qos(*qos_policies, writer_qos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting data writer QoS"); + return nullptr; + } + + // Creates DataWriter (with publisher name to not change name policy) + info->data_writer_ = publisher->create_datawriter( + topic.topic, + writer_qos, + info->listener_); + + if (!info->data_writer_) { + RMW_SET_ERROR_MSG("create_publisher() could not create data writer"); + return nullptr; + } + + // lambda to delete datawriter + auto cleanup_datawriter = rcpputils::make_scope_exit( + [publisher, info]() { + publisher->delete_datawriter(info->data_writer_); + }); + + ///// + // Create RMW GID + info->publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->data_writer_->guid()); + + ///// + // Allocate publisher + rmw_publisher_t * rmw_publisher = rmw_publisher_allocate(); + if (!rmw_publisher) { + RMW_SET_ERROR_MSG("create_publisher() failed to allocate rmw_publisher"); + return nullptr; + } + auto cleanup_rmw_publisher = rcpputils::make_scope_exit( + [rmw_publisher]() { + rmw_free(const_cast(rmw_publisher->topic_name)); + rmw_publisher_free(rmw_publisher); + }); + + bool has_data_sharing = DataSharingKind::OFF != writer_qos.data_sharing().kind(); + rmw_publisher->can_loan_messages = has_data_sharing && info->type_support_->is_plain(); + 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("create_publisher() failed to allocate memory for rmw_publisher topic name"); + return nullptr; + } + memcpy(const_cast(rmw_publisher->topic_name), topic_name, strlen(topic_name) + 1); + + rmw_publisher->options = *publisher_options; + + topic.should_be_deleted = false; + cleanup_rmw_publisher.cancel(); + cleanup_datawriter.cancel(); + return_type_support.cancel(); + cleanup_info.cancel(); + return rmw_publisher; +} diff --git a/src/rmw_client.cpp b/src/rmw_client.cpp new file mode 100644 index 0000000..b72852a --- /dev/null +++ b/src/rmw_client.cpp @@ -0,0 +1,575 @@ +// 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 "fastdds/dds/core/policy/QosPolicies.hpp" +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" + +#include "fastdds/rtps/resources/ResourceManagement.h" + +#include "rcpputils/scope_exit.hpp" +#include "rcutils/logging_macros.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/rmw.h" +#include "rmw/validate_full_topic_name.h" + +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" + +#include "rosidl_typesupport_introspection_c/identifier.h" + +#include "rmw_fastrtps_shared_cpp/custom_client_info.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/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +#include "client_service_common.hpp" +#include "type_support_common.hpp" +#include "type_support_registry.hpp" + +extern "C" +{ +rmw_client_t * +rmw_create_client( + const rmw_node_t * node, + const rosidl_service_type_support_t * type_supports, + const char * service_name, const rmw_qos_profile_t * qos_policies) +{ + ///// + // Check input parameters + RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + eprosima_fastrtps_identifier, + return nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(service_name, nullptr); + if (0 == strlen(service_name)) { + RMW_SET_ERROR_MSG("service_name argument is an empty string"); + return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + if (!qos_policies->avoid_ros_namespace_conventions) { + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(service_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; + } + if (RMW_TOPIC_VALID != validation_result) { + const char * reason = rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("service_name argument is invalid: %s", reason); + return nullptr; + } + } + + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_client() called with invalid QoS"); + return nullptr; + } + + ///// + // Get Participant and SubEntities + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + + ///// + // Get RMW Type Support + const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + type_support = get_service_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; + } + } + + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topics and types + + // Create Topic and Type names + const void * untyped_request_members; + const void * untyped_response_members; + + untyped_request_members = get_request_ptr( + type_support->data, type_support->typesupport_identifier); + untyped_response_members = get_response_ptr( + type_support->data, type_support->typesupport_identifier); + + std::string request_type_name = _create_type_name( + untyped_request_members, type_support->typesupport_identifier); + std::string response_type_name = _create_type_name( + untyped_response_members, type_support->typesupport_identifier); + + std::string response_topic_name = _create_topic_name( + qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + std::string request_topic_name = _create_topic_name( + qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); + + // Get request topic and type + eprosima::fastdds::dds::TypeSupport request_fastdds_type; + eprosima::fastdds::dds::TopicDescription * request_topic_desc = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + request_topic_name, + request_type_name, + &request_topic_desc, + &request_fastdds_type)) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_client() called for existing request topic name %s with incompatible type %s", + request_topic_name.c_str(), request_type_name.c_str()); + return nullptr; + } + + // Get response topic and type + eprosima::fastdds::dds::TypeSupport response_fastdds_type; + eprosima::fastdds::dds::TopicDescription * response_topic_desc = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + response_topic_name, + response_type_name, + &response_topic_desc, + &response_fastdds_type)) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_client() called for existing response topic name %s with incompatible type %s", + response_topic_name.c_str(), response_type_name.c_str()); + return nullptr; + } + + ///// + // Create the custom Client struct (info) + CustomClientInfo * info = new (std::nothrow) CustomClientInfo(); + if (!info) { + RMW_SET_ERROR_MSG("create_client() failed to allocate custom info"); + return nullptr; + } + + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() { + delete info->pub_listener_; + delete info->listener_; + if (info->response_type_support_) { + dds_participant->unregister_type(info->response_type_support_.get_type_name()); + } + if (info->request_type_support_) { + dds_participant->unregister_type(info->request_type_support_.get_type_name()); + } + delete info; + }); + + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->request_publisher_matched_count_ = 0; + info->response_subscriber_matched_count_ = 0; + + ///// + // Create the Type Support structs + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto request_type_impl = type_registry.get_request_type_support(type_support); + if (!request_type_impl) { + RMW_SET_ERROR_MSG("create_client() failed to get request_type_support"); + return nullptr; + } + auto return_request_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { + type_registry.return_request_type_support(type_support); + }); + + auto response_type_impl = type_registry.get_response_type_support(type_support); + if (!response_type_impl) { + RMW_SET_ERROR_MSG("create_client() failed to allocate response type support"); + return nullptr; + } + auto return_response_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { + type_registry.return_response_type_support(type_support); + }); + + info->request_type_support_impl_ = request_type_impl; + info->response_type_support_impl_ = response_type_impl; + + if (!request_fastdds_type) { + auto tsupport = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_client() failed to allocate request TypeSupportProxy"); + return nullptr; + } + + request_fastdds_type.reset(tsupport); + } + + if (!response_fastdds_type) { + auto tsupport = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_client() failed to allocate response TypeSupportProxy"); + return nullptr; + } + + response_fastdds_type.reset(tsupport); + } + + if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(dds_participant)) { + RMW_SET_ERROR_MSG("create_client() failed to register request type"); + return nullptr; + } + info->request_type_support_ = request_fastdds_type; + + if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(dds_participant)) { + RMW_SET_ERROR_MSG("create_client() failed to register response type"); + return nullptr; + } + info->response_type_support_ = response_fastdds_type; + + ///// + // Create Listeners + info->listener_ = new (std::nothrow) ClientListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_client() failed to create response subscriber listener"); + return nullptr; + } + + info->pub_listener_ = new (std::nothrow) ClientPubListener(info); + if (!info->pub_listener_) { + RMW_SET_ERROR_MSG("create_client() failed to create request publisher listener"); + return nullptr; + } + + ///// + // Create and register Topics + // Same default topic QoS for both topics + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { + RMW_SET_ERROR_MSG("create_client() failed setting topic QoS"); + return nullptr; + } + + // Create response topic + rmw_fastrtps_shared_cpp::TopicHolder response_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, response_topic_desc, + response_topic_name, response_type_name, topic_qos, false, &response_topic)) + { + RMW_SET_ERROR_MSG("create_client() failed to create response topic"); + return nullptr; + } + + response_topic_desc = response_topic.desc; + + // Create request topic + rmw_fastrtps_shared_cpp::TopicHolder request_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, request_topic_desc, + request_topic_name, request_type_name, topic_qos, true, &request_topic)) + { + RMW_SET_ERROR_MSG("create_client() failed to create request topic"); + return nullptr; + } + + info->request_topic_ = request_topic_name; + info->response_topic_ = response_topic_name; + + // Keyword to find DataWrtier and DataReader QoS + const std::string topic_name_fallback = "client"; + + ///// + // Create response DataReader + + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataReader QoS with a subscriber profile + // located based on topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "client" is attempted. Else, use the default Fast DDS QoS. + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); + + // Try to load the profile named "client", + // if it does not exist it tries with the response topic name + // It does not need to check the return code, as if any of the profile does not exist, + // the QoS is already set correctly: + // If none exist is default, if only one exists is the one chosen, + // if both exist topic name is chosen + subscriber->get_datareader_qos_from_profile(topic_name_fallback, reader_qos); + subscriber->get_datareader_qos_from_profile(response_topic_name, reader_qos); + + if (!participant_info->leave_middleware_default_qos) { + reader_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + reader_qos.data_sharing().off(); + } + + if (!get_datareader_qos(*qos_policies, reader_qos)) { + RMW_SET_ERROR_MSG("create_client() failed setting response DataReader QoS"); + return nullptr; + } + + // Creates DataReader + info->response_reader_ = subscriber->create_datareader( + response_topic_desc, + reader_qos, + info->listener_); + + if (!info->response_reader_) { + RMW_SET_ERROR_MSG("create_client() failed to create response DataReader"); + return nullptr; + } + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->response_reader_); + }); + + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataWriter QoS with a publisher profile + // located based on topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "client" is attempted. Else, use the default Fast DDS QoS. + eprosima::fastdds::dds::DataWriterQos writer_qos = publisher->get_default_datawriter_qos(); + + // Try to load the profile named "client", + // if it does not exist it tries with the request topic name + // It does not need to check the return code, as if any of the profile does not exist, + // the QoS is already set correctly: + // If none exist is default, if only one exists is the one chosen, + // if both exist topic name is chosen + publisher->get_datawriter_qos_from_profile(topic_name_fallback, writer_qos); + publisher->get_datawriter_qos_from_profile(request_topic_name, writer_qos); + + // Modify specific DataWriter Qos + if (!participant_info->leave_middleware_default_qos) { + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { + writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } + + writer_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + writer_qos.data_sharing().off(); + } + + if (!get_datawriter_qos(*qos_policies, writer_qos)) { + RMW_SET_ERROR_MSG("create_client() failed setting request DataWriter QoS"); + return nullptr; + } + + // Creates DataWriter + info->request_writer_ = publisher->create_datawriter( + request_topic.topic, + writer_qos, + info->pub_listener_); + + if (!info->request_writer_) { + RMW_SET_ERROR_MSG("create_client() failed to create request DataWriter"); + return nullptr; + } + + // lambda to delete datawriter + auto cleanup_datawriter = rcpputils::make_scope_exit( + [publisher, info]() { + publisher->delete_datawriter(info->request_writer_); + }); + + ///// + // Create client + + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "************ Client Details *********"); + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "Sub Topic %s", response_topic_name.c_str()); + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "Pub Topic %s", request_topic_name.c_str()); + RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_dynamic_cpp", "***********"); + + info->writer_guid_ = info->request_writer_->guid(); + info->reader_guid_ = info->response_reader_->guid(); + + rmw_client_t * rmw_client = rmw_client_allocate(); + if (!rmw_client) { + RMW_SET_ERROR_MSG("create_client() failed to allocate memory for rmw_client"); + return nullptr; + } + auto cleanup_rmw_client = rcpputils::make_scope_exit( + [rmw_client]() { + rmw_free(const_cast(rmw_client->service_name)); + rmw_free(rmw_client); + }); + + rmw_client->implementation_identifier = eprosima_fastrtps_identifier; + rmw_client->data = info; + rmw_client->service_name = reinterpret_cast( + rmw_allocate(strlen(service_name) + 1)); + if (!rmw_client->service_name) { + RMW_SET_ERROR_MSG("create_client() failed to allocate memory for service name"); + return nullptr; + } + 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 request_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_writer_->guid()); + common_context->graph_cache.associate_writer( + request_publisher_gid, + common_context->gid, + node->name, + node->namespace_); + + rmw_gid_t response_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_reader_->guid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_reader( + response_subscriber_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) { + common_context->graph_cache.dissociate_reader( + response_subscriber_gid, + common_context->gid, + node->name, + node->namespace_); + common_context->graph_cache.dissociate_writer( + request_publisher_gid, + common_context->gid, + node->name, + node->namespace_); + return nullptr; + } + } + + request_topic.should_be_deleted = false; + response_topic.should_be_deleted = false; + cleanup_rmw_client.cancel(); + cleanup_datawriter.cancel(); + cleanup_datareader.cancel(); + return_response_type_support.cancel(); + return_request_type_support.cancel(); + cleanup_info.cancel(); + return rmw_client; +} + +rmw_ret_t +rmw_destroy_client(rmw_node_t * node, rmw_client_t * client) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + client, + client->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + + auto info = static_cast(client->data); + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + + auto impl = + static_cast(const_cast(info-> + request_type_support_impl_)); + auto ros_type_support = + static_cast(impl->ros_type_support()); + type_registry.return_request_type_support(ros_type_support); + + impl = + static_cast(const_cast(info-> + response_type_support_impl_)); + ros_type_support = static_cast(impl->ros_type_support()); + type_registry.return_response_type_support(ros_type_support); + + return rmw_fastrtps_shared_cpp::__rmw_destroy_client( + eprosima_fastrtps_identifier, node, client); +} + +rmw_ret_t +rmw_client_request_publisher_get_actual_qos( + const rmw_client_t * client, + rmw_qos_profile_t * qos) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + client, + client->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); + + return rmw_fastrtps_shared_cpp::__rmw_client_request_publisher_get_actual_qos(client, qos); +} + +rmw_ret_t +rmw_client_response_subscription_get_actual_qos( + const rmw_client_t * client, + rmw_qos_profile_t * qos) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + client, + client->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); + + return rmw_fastrtps_shared_cpp::__rmw_client_response_subscription_get_actual_qos(client, qos); +} +} // extern "C" diff --git a/src/rmw_compare_gids_equal.cpp b/src/rmw_compare_gids_equal.cpp new file mode 100644 index 0000000..9ebeb4b --- /dev/null +++ b/src/rmw_compare_gids_equal.cpp @@ -0,0 +1,31 @@ +// 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/rmw.h" +#include "rmw/error_handling.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_compare_gids_equal(const rmw_gid_t * gid1, const rmw_gid_t * gid2, bool * result) +{ + return rmw_fastrtps_shared_cpp::__rmw_compare_gids_equal( + eprosima_fastrtps_identifier, gid1, gid2, result); +} +} // extern "C" diff --git a/src/rmw_count.cpp b/src/rmw_count.cpp new file mode 100644 index 0000000..c5dacef --- /dev/null +++ b/src/rmw_count.cpp @@ -0,0 +1,50 @@ +// 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 + +#include "rcutils/logging_macros.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_count_publishers( + const rmw_node_t * node, + const char * topic_name, + size_t * count) +{ + return rmw_fastrtps_shared_cpp::__rmw_count_publishers( + eprosima_fastrtps_identifier, node, topic_name, count); +} + +rmw_ret_t +rmw_count_subscribers( + const rmw_node_t * node, + const char * topic_name, + size_t * count) +{ + return rmw_fastrtps_shared_cpp::__rmw_count_subscribers( + eprosima_fastrtps_identifier, node, topic_name, count); +} +} // extern "C" diff --git a/src/rmw_event.cpp b/src/rmw_event.cpp new file mode 100644 index 0000000..c4b5a72 --- /dev/null +++ b/src/rmw_event.cpp @@ -0,0 +1,50 @@ +// Copyright 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_publisher_event_init( + rmw_event_t * rmw_event, + const rmw_publisher_t * publisher, + rmw_event_type_t event_type) +{ + return rmw_fastrtps_shared_cpp::__rmw_init_event( + eprosima_fastrtps_identifier, + rmw_event, + publisher->implementation_identifier, + publisher->data, + event_type); +} + +rmw_ret_t +rmw_subscription_event_init( + rmw_event_t * rmw_event, + const rmw_subscription_t * subscription, + rmw_event_type_t event_type) +{ + return rmw_fastrtps_shared_cpp::__rmw_init_event( + eprosima_fastrtps_identifier, + rmw_event, + subscription->implementation_identifier, + subscription->data, + event_type); +} +} // extern "C" diff --git a/src/rmw_features.cpp b/src/rmw_features.cpp new file mode 100644 index 0000000..13745a7 --- /dev/null +++ b/src/rmw_features.cpp @@ -0,0 +1,27 @@ +// Copyright 2022 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/features.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +extern "C" +{ + +bool +rmw_feature_supported(rmw_feature_t feature) +{ + return rmw_fastrtps_shared_cpp::__rmw_feature_supported(feature); +} +} // extern "C" diff --git a/src/rmw_get_endpoint_network_flow.cpp b/src/rmw_get_endpoint_network_flow.cpp new file mode 100644 index 0000000..7740818 --- /dev/null +++ b/src/rmw_get_endpoint_network_flow.cpp @@ -0,0 +1,46 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/types.h" +#include "rmw/get_network_flow_endpoints.h" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +extern "C" +{ +rmw_ret_t +rmw_publisher_get_network_flow_endpoints( + const rmw_publisher_t * publisher, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) +{ + return rmw_fastrtps_shared_cpp::__rmw_publisher_get_network_flow_endpoints( + publisher, + allocator, + network_flow_endpoint_array); +} + +rmw_ret_t +rmw_subscription_get_network_flow_endpoints( + const rmw_subscription_t * subscription, + rcutils_allocator_t * allocator, + rmw_network_flow_endpoint_array_t * network_flow_endpoint_array) +{ + return rmw_fastrtps_shared_cpp::__rmw_subscription_get_network_flow_endpoints( + subscription, + allocator, + network_flow_endpoint_array); +} +} // extern "C" diff --git a/src/rmw_get_gid_for_publisher.cpp b/src/rmw_get_gid_for_publisher.cpp new file mode 100644 index 0000000..0bade66 --- /dev/null +++ b/src/rmw_get_gid_for_publisher.cpp @@ -0,0 +1,31 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_gid_for_publisher( + eprosima_fastrtps_identifier, publisher, gid); +} +} // extern "C" diff --git a/src/rmw_get_implementation_identifier.cpp b/src/rmw_get_implementation_identifier.cpp new file mode 100644 index 0000000..94434b3 --- /dev/null +++ b/src/rmw_get_implementation_identifier.cpp @@ -0,0 +1,26 @@ +// 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/rmw.h" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +const char * +rmw_get_implementation_identifier() +{ + return eprosima_fastrtps_identifier; +} +} // extern "C" diff --git a/src/rmw_get_serialization_format.cpp b/src/rmw_get_serialization_format.cpp new file mode 100644 index 0000000..7978b1e --- /dev/null +++ b/src/rmw_get_serialization_format.cpp @@ -0,0 +1,26 @@ +// Copyright 2018 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/rmw.h" + +#include "rmw_fastrtps_dynamic_cpp/serialization_format.hpp" + +extern "C" +{ +const char * +rmw_get_serialization_format() +{ + return eprosima_fastrtps_dynamic_serialization_format; +} +} // extern "C" diff --git a/src/rmw_get_topic_endpoint_info.cpp b/src/rmw_get_topic_endpoint_info.cpp new file mode 100644 index 0000000..d19492e --- /dev/null +++ b/src/rmw_get_topic_endpoint_info.cpp @@ -0,0 +1,46 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw/get_topic_endpoint_info.h" +#include "rmw/topic_endpoint_info_array.h" +#include "rmw/types.h" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +extern "C" +{ +rmw_ret_t +rmw_get_publishers_info_by_topic( + 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 rmw_fastrtps_shared_cpp::__rmw_get_publishers_info_by_topic( + eprosima_fastrtps_identifier, node, allocator, topic_name, no_mangle, publishers_info); +} + +rmw_ret_t +rmw_get_subscriptions_info_by_topic( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * subscriptions_info) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_subscriptions_info_by_topic( + eprosima_fastrtps_identifier, node, allocator, topic_name, no_mangle, subscriptions_info); +} +} // extern "C" diff --git a/src/rmw_guard_condition.cpp b/src/rmw_guard_condition.cpp new file mode 100644 index 0000000..a7420fe --- /dev/null +++ b/src/rmw_guard_condition.cpp @@ -0,0 +1,45 @@ +// 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/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_guard_condition_t * +rmw_create_guard_condition(rmw_context_t * context) +{ + 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_guard_condition( + eprosima_fastrtps_identifier); +} + +rmw_ret_t +rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) +{ + return rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + guard_condition); +} +} // extern "C" diff --git a/src/rmw_init.cpp b/src/rmw_init.cpp new file mode 100644 index 0000000..af621b2 --- /dev/null +++ b/src/rmw_init.cpp @@ -0,0 +1,167 @@ +// 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 +#include +#include + +#include "rcutils/strdup.h" +#include "rcutils/types.h" + +#include "rmw/error_handling.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 "rcpputils/scope_exit.hpp" + +#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) +{ + 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) +{ + 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) +{ + 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) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG( + options->implementation_identifier, + "expected initialized init options", + return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + options, + options->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_FOR_NULL_WITH_MSG( + options->enclave, + "expected non-null enclave", + return RMW_RET_INVALID_ARGUMENT); + if (NULL != context->implementation_identifier) { + RMW_SET_ERROR_MSG("expected a zero-initialized context"); + return RMW_RET_INVALID_ARGUMENT; + } + + auto restore_context = rcpputils::make_scope_exit( + [context]() {*context = rmw_get_zero_initialized_context();}); + + context->instance_id = options->instance_id; + context->implementation_identifier = eprosima_fastrtps_identifier; + context->actual_domain_id = + RMW_DEFAULT_DOMAIN_ID == options->domain_id ? 0uL : options->domain_id; + + context->impl = new (std::nothrow) rmw_context_impl_t(); + if (nullptr == context->impl) { + RMW_SET_ERROR_MSG("failed to allocate context impl"); + return RMW_RET_BAD_ALLOC; + } + auto cleanup_impl = rcpputils::make_scope_exit( + [context]() {delete context->impl;}); + + context->impl->is_shutdown = false; + context->options = rmw_get_zero_initialized_init_options(); + rmw_ret_t ret = rmw_init_options_copy(options, &context->options); + if (RMW_RET_OK != ret) { + return ret; + } + + cleanup_impl.cancel(); + restore_context.cancel(); + return RMW_RET_OK; +} + +rmw_ret_t +rmw_shutdown(rmw_context_t * context) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG( + context->impl, + "expected initialized context", + return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + context, + context->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + context->impl->is_shutdown = true; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_context_fini(rmw_context_t * context) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_FOR_NULL_WITH_MSG( + context->impl, + "expected initialized context", + return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + context, + context->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!context->impl->is_shutdown) { + RCUTILS_SET_ERROR_MSG("context has not been shutdown"); + return RMW_RET_INVALID_ARGUMENT; + } + if (context->impl->count > 0) { + RMW_SET_ERROR_MSG("Finalizing a context with active nodes"); + return RMW_RET_ERROR; + } + rmw_ret_t ret = rmw_init_options_fini(&context->options); + delete context->impl; + *context = rmw_get_zero_initialized_context(); + return ret; +} +} // extern "C" diff --git a/src/rmw_logging.cpp b/src/rmw_logging.cpp new file mode 100644 index 0000000..6f793ff --- /dev/null +++ b/src/rmw_logging.cpp @@ -0,0 +1,29 @@ +// 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/rmw.h" +#include "rmw/error_handling.h" + +#include "rcutils/logging_macros.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +extern "C" +{ +rmw_ret_t +rmw_set_log_severity(rmw_log_severity_t severity) +{ + return rmw_fastrtps_shared_cpp::__rmw_set_log_severity(severity); +} +} // extern "C" diff --git a/src/rmw_node.cpp b/src/rmw_node.cpp new file mode 100644 index 0000000..70a8bb1 --- /dev/null +++ b/src/rmw_node.cpp @@ -0,0 +1,123 @@ +// 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 +#include +#include +#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/rmw.h" + +#include "rmw_fastrtps_shared_cpp/init_rmw_context_impl.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/init_rmw_context_impl.hpp" + +extern "C" +{ +rmw_node_t * +rmw_create_node( + rmw_context_t * context, + const char * name, + const char * namespace_) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(context, nullptr); + 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 nullptr); + RMW_CHECK_FOR_NULL_WITH_MSG( + context->impl, + "expected initialized context", + return nullptr); + if (context->impl->is_shutdown) { + RCUTILS_SET_ERROR_MSG("context has been shutdown"); + return nullptr; + } + + 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) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + rmw_context_t * context = node->context; + + rmw_ret_t ret = RMW_RET_OK; + rmw_error_state_t error_state; + rmw_ret_t inner_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_node( + eprosima_fastrtps_identifier, node); + if (RMW_RET_OK != ret) { + error_state = *rmw_get_error_state(); + ret = inner_ret; + rmw_reset_error(); + } + + inner_ret = rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(context); + if (RMW_RET_OK != inner_ret) { + if (RMW_RET_OK != ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + } else { + error_state = *rmw_get_error_state(); + ret = inner_ret; + } + rmw_reset_error(); + } + + if (RMW_RET_OK != ret) { + rmw_set_error_state(error_state.message, error_state.file, error_state.line_number); + } + + return ret; +} + +const rmw_guard_condition_t * +rmw_node_get_graph_guard_condition(const rmw_node_t * node) +{ + return rmw_fastrtps_shared_cpp::__rmw_node_get_graph_guard_condition( + node); +} +} // extern "C" diff --git a/src/rmw_node_info_and_types.cpp b/src/rmw_node_info_and_types.cpp new file mode 100644 index 0000000..6867628 --- /dev/null +++ b/src/rmw_node_info_and_types.cpp @@ -0,0 +1,100 @@ +// Copyright 2018 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw/allocators.h" +#include "rmw/convert_rcutils_ret_to_rmw_ret.h" +#include "rmw/error_handling.h" +#include "rmw/get_node_info_and_types.h" +#include "rmw/names_and_types.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +// The extern "C" here enforces that overloading is not used. +extern "C" +{ +rmw_ret_t +rmw_get_subscriber_names_and_types_by_node( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_subscriber_names_and_types_by_node( + eprosima_fastrtps_identifier, + node, + allocator, + node_name, + node_namespace, + no_demangle, + topic_names_and_types); +} + +rmw_ret_t +rmw_get_publisher_names_and_types_by_node( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_publisher_names_and_types_by_node( + eprosima_fastrtps_identifier, + node, + allocator, + node_name, + node_namespace, + no_demangle, + topic_names_and_types); +} + +rmw_ret_t +rmw_get_service_names_and_types_by_node( + 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) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_service_names_and_types_by_node( + eprosima_fastrtps_identifier, + node, + allocator, + node_name, + node_namespace, + service_names_and_types); +} + +rmw_ret_t +rmw_get_client_names_and_types_by_node( + 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) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_client_names_and_types_by_node( + eprosima_fastrtps_identifier, + node, + allocator, + node_name, + node_namespace, + service_names_and_types); +} +} // extern "C" diff --git a/src/rmw_node_names.cpp b/src/rmw_node_names.cpp new file mode 100644 index 0000000..715972b --- /dev/null +++ b/src/rmw_node_names.cpp @@ -0,0 +1,53 @@ +// 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 "rcutils/allocator.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/rmw.h" +#include "rmw/sanity_checks.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_get_node_names( + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces) +{ + 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_enclaves( + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * enclaves) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_node_names_with_enclaves( + eprosima_fastrtps_identifier, node, node_names, node_namespaces, enclaves); +} +} // extern "C" diff --git a/src/rmw_publish.cpp b/src/rmw_publish.cpp new file mode 100644 index 0000000..c3c3965 --- /dev/null +++ b/src/rmw_publish.cpp @@ -0,0 +1,57 @@ +// 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 "fastcdr/Cdr.h" +#include "fastcdr/FastBuffer.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_publish( + const rmw_publisher_t * publisher, + const void * ros_message, + rmw_publisher_allocation_t * allocation) +{ + return rmw_fastrtps_shared_cpp::__rmw_publish( + eprosima_fastrtps_identifier, publisher, ros_message, allocation); +} + +rmw_ret_t +rmw_publish_loaned_message( + const rmw_publisher_t * publisher, + void * ros_message, + rmw_publisher_allocation_t * allocation) +{ + return rmw_fastrtps_shared_cpp::__rmw_publish_loaned_message( + eprosima_fastrtps_identifier, publisher, ros_message, allocation); +} + +rmw_ret_t +rmw_publish_serialized_message( + const rmw_publisher_t * publisher, + const rmw_serialized_message_t * serialized_message, + rmw_publisher_allocation_t * allocation) +{ + return rmw_fastrtps_shared_cpp::__rmw_publish_serialized_message( + eprosima_fastrtps_identifier, publisher, serialized_message, allocation); +} +} // extern "C" diff --git a/src/rmw_publisher.cpp b/src/rmw_publisher.cpp new file mode 100644 index 0000000..9eb2c6c --- /dev/null +++ b/src/rmw_publisher.cpp @@ -0,0 +1,222 @@ +// 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/impl/cpp/macros.hpp" + +#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/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" + +extern "C" +{ +rmw_ret_t +rmw_init_publisher_allocation( + const rosidl_message_type_support_t * type_support, + const rosidl_runtime_c__Sequence__bound * message_bounds, + rmw_publisher_allocation_t * allocation) +{ + // Unused in current implementation. + (void) type_support; + (void) message_bounds; + (void) allocation; + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_UNSUPPORTED; +} + +rmw_ret_t +rmw_fini_publisher_allocation(rmw_publisher_allocation_t * allocation) +{ + // Unused in current implementation. + (void) allocation; + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_UNSUPPORTED; +} + +rmw_publisher_t * +rmw_create_publisher( + const rmw_node_t * node, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_publisher_options_t * publisher_options) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + eprosima_fastrtps_identifier, + return nullptr); + + auto participant_info = + static_cast(node->context->impl->participant_info); + rmw_publisher_t * publisher = rmw_fastrtps_dynamic_cpp::create_publisher( + participant_info, + type_supports, + topic_name, + qos_policies, + publisher_options, + false, + true); + + if (!publisher) { + return nullptr; + } + + 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.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_error_state_t error_state = *rmw_get_error_state(); + rmw_reset_error(); + static_cast(common_context->graph_cache.dissociate_writer( + info->publisher_gid, common_context->gid, node->name, node->namespace_)); + rmw_ret = rmw_fastrtps_shared_cpp::destroy_publisher( + eprosima_fastrtps_identifier, participant_info, publisher); + if (RMW_RET_OK != rmw_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "' cleanup\n"); + rmw_reset_error(); + } + rmw_set_error_state(error_state.message, error_state.file, error_state.line_number); + return nullptr; + } + } + return publisher; +} + +rmw_ret_t +rmw_publisher_count_matched_subscriptions( + const rmw_publisher_t * publisher, + size_t * subscription_count) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, + publisher->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription_count, RMW_RET_INVALID_ARGUMENT); + + return rmw_fastrtps_shared_cpp::__rmw_publisher_count_matched_subscriptions( + publisher, subscription_count); +} + +rmw_ret_t +rmw_publisher_assert_liveliness(const rmw_publisher_t * publisher) +{ + return rmw_fastrtps_shared_cpp::__rmw_publisher_assert_liveliness( + eprosima_fastrtps_identifier, publisher); +} + +rmw_ret_t +rmw_publisher_wait_for_all_acked(const rmw_publisher_t * publisher, rmw_time_t wait_timeout) +{ + return rmw_fastrtps_shared_cpp::__rmw_publisher_wait_for_all_acked( + eprosima_fastrtps_identifier, publisher, wait_timeout); +} + +rmw_ret_t +rmw_publisher_get_actual_qos( + const rmw_publisher_t * publisher, + rmw_qos_profile_t * qos) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, + publisher->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); + + return rmw_fastrtps_shared_cpp::__rmw_publisher_get_actual_qos( + publisher, qos); +} + +rmw_ret_t +rmw_borrow_loaned_message( + const rmw_publisher_t * publisher, + const rosidl_message_type_support_t * type_support, + void ** ros_message) +{ + return rmw_fastrtps_shared_cpp::__rmw_borrow_loaned_message( + eprosima_fastrtps_identifier, publisher, type_support, ros_message); +} + +rmw_ret_t +rmw_return_loaned_message_from_publisher( + const rmw_publisher_t * publisher, + void * loaned_message) +{ + return rmw_fastrtps_shared_cpp::__rmw_return_loaned_message_from_publisher( + eprosima_fastrtps_identifier, publisher, loaned_message); +} + +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; + +rmw_ret_t +rmw_destroy_publisher(rmw_node_t * node, rmw_publisher_t * publisher) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, + publisher->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + auto info = static_cast(publisher->data); + auto impl = static_cast(info->type_support_impl_); + auto ros_type_support = static_cast( + impl->ros_type_support()); + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + type_registry.return_message_type_support(ros_type_support); + + return rmw_fastrtps_shared_cpp::__rmw_destroy_publisher( + eprosima_fastrtps_identifier, node, publisher); +} +} // extern "C" diff --git a/src/rmw_qos.cpp b/src/rmw_qos.cpp new file mode 100644 index 0000000..ed42def --- /dev/null +++ b/src/rmw_qos.cpp @@ -0,0 +1,30 @@ +// Copyright 2021 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/rmw_common.hpp" + +extern "C" +{ +rmw_ret_t +rmw_qos_profile_check_compatible( + const rmw_qos_profile_t publisher_profile, + const rmw_qos_profile_t subscription_profile, + rmw_qos_compatibility_type_t * compatibility, + char * reason, + size_t reason_size) +{ + return rmw_fastrtps_shared_cpp::__rmw_qos_profile_check_compatible( + publisher_profile, subscription_profile, compatibility, reason, reason_size); +} +} // extern "C" diff --git a/src/rmw_request.cpp b/src/rmw_request.cpp new file mode 100644 index 0000000..90d13a5 --- /dev/null +++ b/src/rmw_request.cpp @@ -0,0 +1,45 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_send_request( + const rmw_client_t * client, + const void * ros_request, + int64_t * sequence_id) +{ + return rmw_fastrtps_shared_cpp::__rmw_send_request( + eprosima_fastrtps_identifier, client, ros_request, sequence_id); +} + +rmw_ret_t +rmw_take_request( + const rmw_service_t * service, + rmw_service_info_t * request_header, + void * ros_request, + bool * taken) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_request( + eprosima_fastrtps_identifier, service, request_header, ros_request, taken); +} +} // extern "C" diff --git a/src/rmw_response.cpp b/src/rmw_response.cpp new file mode 100644 index 0000000..9a424a5 --- /dev/null +++ b/src/rmw_response.cpp @@ -0,0 +1,44 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_take_response( + const rmw_client_t * client, + rmw_service_info_t * request_header, + void * ros_response, + bool * taken) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_response( + eprosima_fastrtps_identifier, client, request_header, ros_response, taken); +} + +rmw_ret_t +rmw_send_response( + const rmw_service_t * service, + rmw_request_id_t * request_header, + void * ros_response) +{ + return rmw_fastrtps_shared_cpp::__rmw_send_response( + eprosima_fastrtps_identifier, service, request_header, ros_response); +} +} // extern "C" diff --git a/src/rmw_serialize.cpp b/src/rmw_serialize.cpp new file mode 100644 index 0000000..c6d28b0 --- /dev/null +++ b/src/rmw_serialize.cpp @@ -0,0 +1,105 @@ +// 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 "fastcdr/FastBuffer.h" + +#include "rmw/error_handling.h" +#include "rmw/serialized_message.h" +#include "rmw/rmw.h" + +#include "./type_support_common.hpp" +#include "./type_support_registry.hpp" + +extern "C" +{ +rmw_ret_t +rmw_serialize( + const void * ros_message, + const rosidl_message_type_support_t * type_support, + rmw_serialized_message_t * serialized_message) +{ + const rosidl_message_type_support_t * ts = get_message_typesupport_handle( + type_support, rosidl_typesupport_introspection_c__identifier); + if (!ts) { + ts = get_message_typesupport_handle( + type_support, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!ts) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return RMW_RET_ERROR; + } + } + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto tss = type_registry.get_message_type_support(ts); + auto data_length = tss->getEstimatedSerializedSize(ros_message, ts->data); + if (serialized_message->buffer_capacity < data_length) { + if (rmw_serialized_message_resize(serialized_message, data_length) != RMW_RET_OK) { + RMW_SET_ERROR_MSG("unable to dynamically resize serialized message"); + type_registry.return_message_type_support(ts); + return RMW_RET_ERROR; + } + } + + eprosima::fastcdr::FastBuffer buffer( + reinterpret_cast(serialized_message->buffer), + data_length); + eprosima::fastcdr::Cdr ser( + buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + + auto ret = tss->serializeROSmessage(ros_message, ser, ts->data); + serialized_message->buffer_length = data_length; + serialized_message->buffer_capacity = data_length; + type_registry.return_message_type_support(ts); + return ret == true ? RMW_RET_OK : RMW_RET_ERROR; +} + +rmw_ret_t +rmw_deserialize( + const rmw_serialized_message_t * serialized_message, + const rosidl_message_type_support_t * type_support, + void * ros_message) +{ + const rosidl_message_type_support_t * ts = get_message_typesupport_handle( + type_support, rosidl_typesupport_introspection_c__identifier); + if (!ts) { + ts = get_message_typesupport_handle( + type_support, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!ts) { + RMW_SET_ERROR_MSG("type support not from this implementation"); + return RMW_RET_ERROR; + } + } + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto tss = type_registry.get_message_type_support(ts); + eprosima::fastcdr::FastBuffer buffer( + reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); + eprosima::fastcdr::Cdr deser(buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + + auto ret = tss->deserializeROSmessage(deser, ros_message, ts->data); + type_registry.return_message_type_support(ts); + return ret == true ? RMW_RET_OK : RMW_RET_ERROR; +} + +rmw_ret_t +rmw_get_serialized_message_size( + const rosidl_message_type_support_t * /*type_support*/, + const rosidl_runtime_c__Sequence__bound * /*message_bounds*/, + size_t * /*size*/) +{ + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_UNSUPPORTED; +} +} // extern "C" diff --git a/src/rmw_service.cpp b/src/rmw_service.cpp new file mode 100644 index 0000000..a0d5f4b --- /dev/null +++ b/src/rmw_service.cpp @@ -0,0 +1,574 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" + +#include "rcpputils/scope_exit.hpp" +#include "rcutils/error_handling.h" +#include "rcutils/logging_macros.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/rmw.h" +#include "rmw/validate_full_topic_name.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#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" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +#include "client_service_common.hpp" +#include "type_support_common.hpp" +#include "type_support_registry.hpp" + +extern "C" +{ +rmw_service_t * +rmw_create_service( + const rmw_node_t * node, + const rosidl_service_type_support_t * type_supports, + const char * service_name, const rmw_qos_profile_t * qos_policies) +{ + ///// + // Check input parameters + RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + eprosima_fastrtps_identifier, + return nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(service_name, nullptr); + if (0 == strlen(service_name)) { + RMW_SET_ERROR_MSG("service_name argument is an empty string"); + return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + if (!qos_policies->avoid_ros_namespace_conventions) { + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(service_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; + } + if (RMW_TOPIC_VALID != validation_result) { + const char * reason = rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("service_name argument is invalid: %s", reason); + return nullptr; + } + } + + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_service() called with invalid QoS"); + return nullptr; + } + + ///// + // Get Participant and SubEntities + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Publisher * publisher = participant_info->publisher_; + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + + ///// + // Get RMW Type Support + const rosidl_service_type_support_t * type_support = get_service_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + type_support = get_service_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; + } + } + + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topics and types + + // Create Topic and Type names + const void * untyped_request_members = get_request_ptr( + type_support->data, type_support->typesupport_identifier); + const void * untyped_response_members = get_response_ptr( + type_support->data, type_support->typesupport_identifier); + + std::string request_type_name = _create_type_name( + untyped_request_members, type_support->typesupport_identifier); + std::string response_type_name = _create_type_name( + untyped_response_members, type_support->typesupport_identifier); + + std::string response_topic_name = _create_topic_name( + qos_policies, ros_service_response_prefix, service_name, "Reply").to_string(); + std::string request_topic_name = _create_topic_name( + qos_policies, ros_service_requester_prefix, service_name, "Request").to_string(); + + // Get request topic and type + eprosima::fastdds::dds::TypeSupport request_fastdds_type; + eprosima::fastdds::dds::TopicDescription * request_topic_desc = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + request_topic_name, + request_type_name, + &request_topic_desc, + &request_fastdds_type)) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_service() called for existing request topic name %s with incompatible type %s", + request_topic_name.c_str(), request_type_name.c_str()); + return nullptr; + } + + // Get response topic and type + eprosima::fastdds::dds::TypeSupport response_fastdds_type; + eprosima::fastdds::dds::TopicDescription * response_topic_desc = nullptr; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + response_topic_name, + response_type_name, + &response_topic_desc, + &response_fastdds_type)) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_service() called for existing response topic name %s with incompatible type %s", + response_topic_name.c_str(), response_type_name.c_str()); + return nullptr; + } + + ///// + // Create the custom Service struct (info) + CustomServiceInfo * info = new (std::nothrow) CustomServiceInfo(); + if (!info) { + RMW_SET_ERROR_MSG("create_service() failed to allocate custom info"); + return nullptr; + } + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() { + delete info->pub_listener_; + delete info->listener_; + if (info->response_type_support_) { + dds_participant->unregister_type(info->response_type_support_.get_type_name()); + } + if (info->request_type_support_) { + dds_participant->unregister_type(info->request_type_support_.get_type_name()); + } + delete info; + }); + + info->typesupport_identifier_ = type_support->typesupport_identifier; + + ///// + // Create the Type Support structs + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto request_type_impl = type_registry.get_request_type_support(type_support); + if (!request_type_impl) { + RMW_SET_ERROR_MSG("create_service() failed to get request_type_support"); + return nullptr; + } + auto return_request_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { + type_registry.return_request_type_support(type_support); + }); + + auto response_type_impl = type_registry.get_response_type_support(type_support); + if (!response_type_impl) { + RMW_SET_ERROR_MSG("create_service() failed to get response_type_support"); + return nullptr; + } + auto return_response_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { + type_registry.return_response_type_support(type_support); + }); + + info->request_type_support_impl_ = request_type_impl; + info->response_type_support_impl_ = response_type_impl; + + if (!request_fastdds_type) { + auto tsupport = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(request_type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_service() failed to allocate request TypeSupportProxy"); + return nullptr; + } + + request_fastdds_type.reset(tsupport); + } + + if (!response_fastdds_type) { + auto tsupport = + new (std::nothrow) rmw_fastrtps_dynamic_cpp::TypeSupportProxy(response_type_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_service() failed to allocate response TypeSupportProxy"); + return nullptr; + } + + response_fastdds_type.reset(tsupport); + } + + if (ReturnCode_t::RETCODE_OK != request_fastdds_type.register_type(dds_participant)) { + RMW_SET_ERROR_MSG("create_service() failed to register request type"); + return nullptr; + } + info->request_type_support_ = request_fastdds_type; + + if (ReturnCode_t::RETCODE_OK != response_fastdds_type.register_type(dds_participant)) { + RMW_SET_ERROR_MSG("create_service() failed to register response type"); + return nullptr; + } + info->response_type_support_ = response_fastdds_type; + + ///// + // Create Listeners + info->listener_ = new (std::nothrow) ServiceListener(info); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_service() failed to create request subscriber listener"); + return nullptr; + } + + info->pub_listener_ = new (std::nothrow) ServicePubListener(info); + if (!info->pub_listener_) { + RMW_SET_ERROR_MSG("create_service() failed to create response publisher listener"); + return nullptr; + } + + ///// + // Create and register Topics + // Same default topic QoS for both topics + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { + RMW_SET_ERROR_MSG("create_service() failed setting topic QoS"); + return nullptr; + } + + // Create request topic + rmw_fastrtps_shared_cpp::TopicHolder request_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, request_topic_desc, + request_topic_name, request_type_name, topic_qos, false, &request_topic)) + { + RMW_SET_ERROR_MSG("create_service() failed to create request topic"); + return nullptr; + } + + request_topic_desc = request_topic.desc; + + // Create response topic + rmw_fastrtps_shared_cpp::TopicHolder response_topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, response_topic_desc, + response_topic_name, response_type_name, topic_qos, true, &response_topic)) + { + RMW_SET_ERROR_MSG("create_service() failed to create response topic"); + return nullptr; + } + + // Keyword to find DataWrtier and DataReader QoS + const std::string topic_name_fallback = "service"; + + ///// + // Create request DataReader + + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataReader QoS with a subscriber profile + // located based on topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "service" is attempted. Else, use the default Fast DDS QoS. + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); + + // Try to load the profile named "service", + // if it does not exist it tries with the request topic name + // It does not need to check the return code, as if any of the profile does not exist, + // the QoS is already set correctly: + // If none exist is default, if only one exists is the one chosen, + // if both exist topic name is chosen + subscriber->get_datareader_qos_from_profile(topic_name_fallback, reader_qos); + subscriber->get_datareader_qos_from_profile(request_topic_name, reader_qos); + + if (!participant_info->leave_middleware_default_qos) { + reader_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + reader_qos.data_sharing().off(); + } + + if (!get_datareader_qos(*qos_policies, reader_qos)) { + RMW_SET_ERROR_MSG("create_service() failed setting request DataReader QoS"); + return nullptr; + } + + // Creates DataReader + info->request_reader_ = subscriber->create_datareader( + request_topic_desc, + reader_qos, + info->listener_); + + if (!info->request_reader_) { + RMW_SET_ERROR_MSG("create_service() failed to create request DataReader"); + return nullptr; + } + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->request_reader_); + }); + + + ///// + // Create response DataWriter + + // If FASTRTPS_DEFAULT_PROFILES_FILE defined, fill DataWriter QoS with a publisher profile + // located based on topic name defined by _create_topic_name(). If no profile is found, a search + // with profile_name "service" is attempted. Else, use the default Fast DDS QoS. + eprosima::fastdds::dds::DataWriterQos writer_qos = publisher->get_default_datawriter_qos(); + + // Try to load the profile named "service", + // if it does not exist it tries with the request topic name + // It does not need to check the return code, as if any of the profile does not exist, + // the QoS is already set correctly: + // If none exist is default, if only one exists is the one chosen, + // if both exist topic name is chosen + publisher->get_datawriter_qos_from_profile(topic_name_fallback, writer_qos); + publisher->get_datawriter_qos_from_profile(response_topic_name, writer_qos); + + // Modify specific DataWriter Qos + if (!participant_info->leave_middleware_default_qos) { + if (participant_info->publishing_mode == publishing_mode_t::ASYNCHRONOUS) { + writer_qos.publish_mode().kind = eprosima::fastrtps::ASYNCHRONOUS_PUBLISH_MODE; + } else if (participant_info->publishing_mode == publishing_mode_t::SYNCHRONOUS) { + writer_qos.publish_mode().kind = eprosima::fastrtps::SYNCHRONOUS_PUBLISH_MODE; + } + + writer_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + writer_qos.data_sharing().off(); + } + + if (!get_datawriter_qos(*qos_policies, writer_qos)) { + RMW_SET_ERROR_MSG("create_service() failed setting response DataWriter QoS"); + return nullptr; + } + + // Creates DataWriter + info->response_writer_ = publisher->create_datawriter( + response_topic.topic, + writer_qos, + info->pub_listener_); + + if (!info->response_writer_) { + RMW_SET_ERROR_MSG("create_service() failed to create response DataWriter"); + return nullptr; + } + + // lambda to delete datawriter + auto cleanup_datawriter = rcpputils::make_scope_exit( + [publisher, info]() { + publisher->delete_datawriter(info->response_writer_); + }); + + ///// + // Create Service + + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "************ Service Details *********"); + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "Sub Topic %s", request_topic_name.c_str()); + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "Pub Topic %s", response_topic_name.c_str()); + RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_dynamic_cpp", "***********"); + + rmw_service_t * rmw_service = rmw_service_allocate(); + if (!rmw_service) { + RMW_SET_ERROR_MSG("create_service() failed to allocate memory for rmw_service"); + return nullptr; + } + auto cleanup_rmw_service = rcpputils::make_scope_exit( + [rmw_service]() { + rmw_free(const_cast(rmw_service->service_name)); + rmw_free(rmw_service); + }); + + rmw_service->implementation_identifier = eprosima_fastrtps_identifier; + rmw_service->data = info; + rmw_service->service_name = reinterpret_cast( + rmw_allocate(strlen(service_name) + 1)); + if (!rmw_service->service_name) { + RMW_SET_ERROR_MSG("create_service() failed to allocate memory for service name"); + return nullptr; + } + 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 request_subscriber_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->request_reader_->guid()); + common_context->graph_cache.associate_reader( + request_subscriber_gid, + common_context->gid, + node->name, + node->namespace_); + + rmw_gid_t response_publisher_gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->response_writer_->guid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.associate_writer( + response_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) { + common_context->graph_cache.dissociate_writer( + response_publisher_gid, + common_context->gid, + node->name, + node->namespace_); + common_context->graph_cache.dissociate_reader( + request_subscriber_gid, + common_context->gid, + node->name, + node->namespace_); + return nullptr; + } + } + + request_topic.should_be_deleted = false; + response_topic.should_be_deleted = false; + cleanup_rmw_service.cancel(); + cleanup_datawriter.cancel(); + cleanup_datareader.cancel(); + return_response_type_support.cancel(); + return_request_type_support.cancel(); + cleanup_info.cancel(); + return rmw_service; +} + +rmw_ret_t +rmw_destroy_service(rmw_node_t * node, rmw_service_t * service) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + service, + service->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + auto info = static_cast(service->data); + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + + auto impl = + static_cast(const_cast(info-> + request_type_support_impl_)); + auto ros_type_support = + static_cast(impl->ros_type_support()); + type_registry.return_request_type_support(ros_type_support); + + impl = + static_cast(const_cast(info-> + response_type_support_impl_)); + ros_type_support = static_cast(impl->ros_type_support()); + type_registry.return_response_type_support(ros_type_support); + + return rmw_fastrtps_shared_cpp::__rmw_destroy_service( + eprosima_fastrtps_identifier, node, service); +} + +rmw_ret_t +rmw_service_response_publisher_get_actual_qos( + const rmw_service_t * service, + rmw_qos_profile_t * qos) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + service, + service->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); + + return rmw_fastrtps_shared_cpp::__rmw_service_response_publisher_get_actual_qos(service, qos); +} + +rmw_ret_t +rmw_service_request_subscription_get_actual_qos( + const rmw_service_t * service, + rmw_qos_profile_t * qos) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + service, + service->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); + + return rmw_fastrtps_shared_cpp::__rmw_service_request_subscription_get_actual_qos(service, qos); +} +} // extern "C" diff --git a/src/rmw_service_names_and_types.cpp b/src/rmw_service_names_and_types.cpp new file mode 100644 index 0000000..d8e839f --- /dev/null +++ b/src/rmw_service_names_and_types.cpp @@ -0,0 +1,37 @@ +// 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/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_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_get_service_names_and_types( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + rmw_names_and_types_t * service_names_and_types) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_service_names_and_types( + eprosima_fastrtps_identifier, node, allocator, service_names_and_types); +} +} // extern "C" diff --git a/src/rmw_service_server_is_available.cpp b/src/rmw_service_server_is_available.cpp new file mode 100644 index 0000000..d4d739a --- /dev/null +++ b/src/rmw_service_server_is_available.cpp @@ -0,0 +1,36 @@ +// 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/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/rmw.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_service_server_is_available( + const rmw_node_t * node, + const rmw_client_t * client, + bool * is_available) +{ + return rmw_fastrtps_shared_cpp::__rmw_service_server_is_available( + eprosima_fastrtps_identifier, node, client, is_available); +} +} // extern "C" diff --git a/src/rmw_subscription.cpp b/src/rmw_subscription.cpp new file mode 100644 index 0000000..d3059c4 --- /dev/null +++ b/src/rmw_subscription.cpp @@ -0,0 +1,212 @@ +// 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 "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/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.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" + +extern "C" +{ +rmw_ret_t +rmw_init_subscription_allocation( + const rosidl_message_type_support_t * type_support, + const rosidl_runtime_c__Sequence__bound * message_bounds, + rmw_subscription_allocation_t * allocation) +{ + // Unused in current implementation. + (void) type_support; + (void) message_bounds; + (void) allocation; + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_UNSUPPORTED; +} + +rmw_ret_t +rmw_fini_subscription_allocation(rmw_subscription_allocation_t * allocation) +{ + // Unused in current implementation. + (void) allocation; + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_UNSUPPORTED; +} + +rmw_subscription_t * +rmw_create_subscription( + const rmw_node_t * node, + const rosidl_message_type_support_t * type_supports, + const char * topic_name, + const rmw_qos_profile_t * qos_policies, + const rmw_subscription_options_t * subscription_options) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, nullptr); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + eprosima_fastrtps_identifier, + return nullptr); + + auto participant_info = + static_cast(node->context->impl->participant_info); + + 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; + } + + 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.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_error_state_t error_state = *rmw_get_error_state(); + rmw_reset_error(); + static_cast(common_context->graph_cache.dissociate_writer( + info->subscription_gid_, common_context->gid, node->name, node->namespace_)); + rmw_ret = rmw_fastrtps_shared_cpp::destroy_subscription( + eprosima_fastrtps_identifier, participant_info, subscription); + if (RMW_RET_OK != rmw_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "' cleanup\n"); + rmw_reset_error(); + } + rmw_set_error_state(error_state.message, error_state.file, error_state.line_number); + return nullptr; + } + } + info->node_ = node; + info->common_context_ = common_context; + + return subscription; +} + +rmw_ret_t +rmw_subscription_count_matched_publishers( + const rmw_subscription_t * subscription, + size_t * publisher_count) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, + subscription->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(publisher_count, RMW_RET_INVALID_ARGUMENT); + + return rmw_fastrtps_shared_cpp::__rmw_subscription_count_matched_publishers( + subscription, publisher_count); +} + +rmw_ret_t +rmw_subscription_get_actual_qos( + const rmw_subscription_t * subscription, + rmw_qos_profile_t * qos) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, + subscription->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(qos, RMW_RET_INVALID_ARGUMENT); + + return rmw_fastrtps_shared_cpp::__rmw_subscription_get_actual_qos(subscription, qos); +} + +rmw_ret_t +rmw_subscription_set_content_filter( + rmw_subscription_t * subscription, + const rmw_subscription_content_filter_options_t * options) +{ + // Unused in current implementation. + (void) subscription; + (void) options; + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_UNSUPPORTED; +} + +rmw_ret_t +rmw_subscription_get_content_filter( + const rmw_subscription_t * subscription, + rcutils_allocator_t * allocator, + rmw_subscription_content_filter_options_t * options) +{ + // Unused in current implementation. + (void) subscription; + (void) allocator; + (void) options; + RMW_SET_ERROR_MSG("unimplemented"); + return RMW_RET_UNSUPPORTED; +} + +using BaseTypeSupport = rmw_fastrtps_dynamic_cpp::BaseTypeSupport; + +rmw_ret_t +rmw_destroy_subscription(rmw_node_t * node, rmw_subscription_t * subscription) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, + subscription->implementation_identifier, + eprosima_fastrtps_identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + auto info = static_cast(subscription->data); + auto impl = static_cast(info->type_support_impl_); + auto ros_type_support = static_cast( + impl->ros_type_support()); + + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + type_registry.return_message_type_support(ros_type_support); + + return rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( + eprosima_fastrtps_identifier, node, subscription); +} +} // extern "C" diff --git a/src/rmw_take.cpp b/src/rmw_take.cpp new file mode 100644 index 0000000..29e3663 --- /dev/null +++ b/src/rmw_take.cpp @@ -0,0 +1,131 @@ +// 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/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/serialized_message.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_take( + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_subscription_allocation_t * allocation) +{ + return rmw_fastrtps_shared_cpp::__rmw_take( + eprosima_fastrtps_identifier, subscription, ros_message, taken, allocation); +} + +rmw_ret_t +rmw_take_with_info( + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_with_info( + eprosima_fastrtps_identifier, subscription, ros_message, taken, message_info, allocation); +} + +rmw_ret_t +rmw_take_sequence( + const rmw_subscription_t * subscription, + size_t count, + rmw_message_sequence_t * message_sequence, + rmw_message_info_sequence_t * message_info_sequence, + size_t * taken, + rmw_subscription_allocation_t * allocation) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_sequence( + eprosima_fastrtps_identifier, subscription, count, message_sequence, message_info_sequence, + taken, allocation); +} + +rmw_ret_t +rmw_take_serialized_message( + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_subscription_allocation_t * allocation) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_serialized_message( + eprosima_fastrtps_identifier, subscription, serialized_message, taken, allocation); +} + +rmw_ret_t +rmw_take_serialized_message_with_info( + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_serialized_message_with_info( + eprosima_fastrtps_identifier, subscription, serialized_message, taken, message_info, + allocation); +} + +rmw_ret_t +rmw_take_loaned_message( + const rmw_subscription_t * subscription, + void ** loaned_message, + bool * taken, + rmw_subscription_allocation_t * allocation) +{ + static_cast(allocation); + return rmw_fastrtps_shared_cpp::__rmw_take_loaned_message_internal( + eprosima_fastrtps_identifier, subscription, loaned_message, taken, nullptr); +} + +rmw_ret_t +rmw_take_loaned_message_with_info( + const rmw_subscription_t * subscription, + void ** loaned_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + static_cast(allocation); + RMW_CHECK_ARGUMENT_FOR_NULL(message_info, RMW_RET_INVALID_ARGUMENT); + return rmw_fastrtps_shared_cpp::__rmw_take_loaned_message_internal( + eprosima_fastrtps_identifier, subscription, loaned_message, taken, message_info); +} + +rmw_ret_t +rmw_return_loaned_message_from_subscription( + const rmw_subscription_t * subscription, + void * loaned_message) +{ + return rmw_fastrtps_shared_cpp::__rmw_return_loaned_message_from_subscription( + eprosima_fastrtps_identifier, subscription, loaned_message); +} + +rmw_ret_t +rmw_take_event( + const rmw_event_t * event_handle, + void * event_info, + bool * taken) +{ + return rmw_fastrtps_shared_cpp::__rmw_take_event( + eprosima_fastrtps_identifier, event_handle, event_info, taken); +} +} // extern "C" diff --git a/src/rmw_topic_names_and_types.cpp b/src/rmw_topic_names_and_types.cpp new file mode 100644 index 0000000..1e104a4 --- /dev/null +++ b/src/rmw_topic_names_and_types.cpp @@ -0,0 +1,49 @@ +// 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 + +#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_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_get_topic_names_and_types( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) +{ + return rmw_fastrtps_shared_cpp::__rmw_get_topic_names_and_types( + eprosima_fastrtps_identifier, node, allocator, no_demangle, topic_names_and_types); +} +} // extern "C" diff --git a/src/rmw_trigger_guard_condition.cpp b/src/rmw_trigger_guard_condition.cpp new file mode 100644 index 0000000..8e0a5c3 --- /dev/null +++ b/src/rmw_trigger_guard_condition.cpp @@ -0,0 +1,30 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition_handle) +{ + return rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + eprosima_fastrtps_identifier, guard_condition_handle); +} +} // extern "C" diff --git a/src/rmw_wait.cpp b/src/rmw_wait.cpp new file mode 100644 index 0000000..d10e086 --- /dev/null +++ b/src/rmw_wait.cpp @@ -0,0 +1,37 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_ret_t +rmw_wait( + rmw_subscriptions_t * subscriptions, + rmw_guard_conditions_t * guard_conditions, + rmw_services_t * services, + rmw_clients_t * clients, + rmw_events_t * events, + rmw_wait_set_t * wait_set, + const rmw_time_t * wait_timeout) +{ + return rmw_fastrtps_shared_cpp::__rmw_wait( + eprosima_fastrtps_identifier, subscriptions, guard_conditions, services, clients, events, + wait_set, wait_timeout); +} +} // extern "C" diff --git a/src/rmw_wait_set.cpp b/src/rmw_wait_set.cpp new file mode 100644 index 0000000..f86f585 --- /dev/null +++ b/src/rmw_wait_set.cpp @@ -0,0 +1,38 @@ +// 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/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +extern "C" +{ +rmw_wait_set_t * +rmw_create_wait_set(rmw_context_t * context, size_t max_conditions) +{ + return rmw_fastrtps_shared_cpp::__rmw_create_wait_set( + eprosima_fastrtps_identifier, context, max_conditions); +} + +rmw_ret_t +rmw_destroy_wait_set(rmw_wait_set_t * wait_set) +{ + return rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( + eprosima_fastrtps_identifier, wait_set); +} +} // extern "C" diff --git a/src/serialization_format.cpp b/src/serialization_format.cpp new file mode 100644 index 0000000..0353497 --- /dev/null +++ b/src/serialization_format.cpp @@ -0,0 +1,17 @@ +// Copyright 2018 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/serialization_format.hpp" + +const char * const eprosima_fastrtps_dynamic_serialization_format = "cdr"; diff --git a/src/subscription.cpp b/src/subscription.cpp new file mode 100644 index 0000000..2637097 --- /dev/null +++ b/src/subscription.cpp @@ -0,0 +1,361 @@ +// 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 "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" + +#include "fastdds/rtps/resources/ResourceManagement.h" + +#include "rcutils/error_handling.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/validate_full_topic_name.h" + +#include "rcpputils/scope_exit.hpp" + +#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 "rmw_fastrtps_shared_cpp/subscription.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" + +#include "fastrtps/participant/Participant.h" +#include "fastrtps/subscriber/Subscriber.h" +#include "fastrtps/xmlparser/XMLProfileManager.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 PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; + +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) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); + + ///// + // Check input parameters + RMW_CHECK_ARGUMENT_FOR_NULL(participant_info, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(type_supports, nullptr); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, nullptr); + if (0 == strlen(topic_name)) { + RMW_SET_ERROR_MSG("create_subscription() called with an empty topic_name argument"); + return nullptr; + } + RMW_CHECK_ARGUMENT_FOR_NULL(qos_policies, nullptr); + if (!qos_policies->avoid_ros_namespace_conventions) { + int validation_result = RMW_TOPIC_VALID; + rmw_ret_t ret = rmw_validate_full_topic_name(topic_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; + } + if (RMW_TOPIC_VALID != validation_result) { + const char * reason = rmw_full_topic_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_subscription() called with invalid topic name: %s", reason); + return nullptr; + } + } + RMW_CHECK_ARGUMENT_FOR_NULL(subscription_options, nullptr); + + ///// + // Check RMW QoS + if (!is_valid_qos(*qos_policies)) { + RMW_SET_ERROR_MSG("create_subscription() called with invalid QoS"); + return nullptr; + } + + ///// + // Get RMW Type Support + const rosidl_message_type_support_t * type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (!type_support) { + rcutils_error_string_t prev_error_string = rcutils_get_error_string(); + rcutils_reset_error(); + type_support = get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_cpp::typesupport_identifier); + if (!type_support) { + rcutils_error_string_t error_string = rcutils_get_error_string(); + rcutils_reset_error(); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Type support not from this implementation. Got:\n" + " %s\n" + " %s\n" + "while fetching it", + prev_error_string.str, error_string.str); + return nullptr; + } + } + + std::lock_guard lck(participant_info->entity_creation_mutex_); + + ///// + // Find and check existing topic and type + + // Create Topic and Type names + std::string type_name = _create_type_name( + type_support->data, type_support->typesupport_identifier); + auto topic_name_mangled = + _create_topic_name(qos_policies, ros_topic_prefix, topic_name).to_string(); + + eprosima::fastdds::dds::TypeSupport fastdds_type; + eprosima::fastdds::dds::TopicDescription * des_topic; + if (!rmw_fastrtps_shared_cpp::find_and_check_topic_and_type( + participant_info, + topic_name_mangled, + type_name, + &des_topic, + &fastdds_type)) + { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "create_subscription() called with existing topic name %s with incompatible type %s", + topic_name_mangled.c_str(), type_name.c_str()); + return nullptr; + } + + ///// + // Get Participant and Subscriber + eprosima::fastdds::dds::DomainParticipant * dds_participant = participant_info->participant_; + eprosima::fastdds::dds::Subscriber * subscriber = participant_info->subscriber_; + + ///// + // Create the custom Subscriber struct (info) + auto info = new (std::nothrow) CustomSubscriberInfo(); + if (!info) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate CustomSubscriberInfo"); + return nullptr; + } + + auto cleanup_info = rcpputils::make_scope_exit( + [info, dds_participant]() { + delete info->listener_; + if (info->type_support_) { + dds_participant->unregister_type(info->type_support_.get_type_name()); + } + delete info; + }); + + ///// + // Create the Type Support struct + TypeSupportRegistry & type_registry = TypeSupportRegistry::get_instance(); + auto type_support_impl = type_registry.get_message_type_support(type_support); + if (!type_support_impl) { + RMW_SET_ERROR_MSG("create_subscription() failed to get message_type_support"); + return nullptr; + } + auto return_type_support = rcpputils::make_scope_exit( + [&type_registry, type_support]() { + type_registry.return_message_type_support(type_support); + }); + + info->typesupport_identifier_ = type_support->typesupport_identifier; + info->type_support_impl_ = type_support_impl; + + if (!fastdds_type) { + auto tsupport = new (std::nothrow) TypeSupportProxy(type_support_impl); + if (!tsupport) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate TypeSupportProxy"); + return nullptr; + } + + // Transfer ownership to fastdds_type + fastdds_type.reset(tsupport); + } + + if (keyed && !fastdds_type->m_isGetKeyDefined) { + RMW_SET_ERROR_MSG("create_subscription() requested a keyed topic with a non-keyed type"); + return nullptr; + } + + if (ReturnCode_t::RETCODE_OK != fastdds_type.register_type(dds_participant)) { + RMW_SET_ERROR_MSG("create_subscription() failed to register type"); + return nullptr; + } + + info->type_support_ = fastdds_type; + + ///// + // Create Listener + if (create_subscription_listener) { + info->listener_ = new (std::nothrow) SubListener(info, qos_policies->depth); + + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscription() could not create subscription listener"); + return nullptr; + } + } + + ///// + // Create and register Topic + eprosima::fastdds::dds::TopicQos topic_qos = dds_participant->get_default_topic_qos(); + if (!get_topic_qos(*qos_policies, topic_qos)) { + RMW_SET_ERROR_MSG("create_publisher() failed setting topic QoS"); + return nullptr; + } + + rmw_fastrtps_shared_cpp::TopicHolder topic; + if (!rmw_fastrtps_shared_cpp::cast_or_create_topic( + dds_participant, des_topic, + topic_name_mangled, type_name, topic_qos, false, &topic)) + { + RMW_SET_ERROR_MSG("create_subscription() failed to create topic"); + return nullptr; + } + + des_topic = topic.desc; + + ///// + // Create DataReader + + // If the user defined an XML file via env "FASTRTPS_DEFAULT_PROFILES_FILE", try to load + // datareader which profile name matches with topic_name. If such profile does not exist, + // then use the default Fast DDS QoS. + eprosima::fastdds::dds::DataReaderQos reader_qos = subscriber->get_default_datareader_qos(); + + // Try to load the profile with the topic name + // It does not need to check the return code, as if the profile does not exist, + // the QoS is already the default + subscriber->get_datareader_qos_from_profile(topic_name, reader_qos); + + if (!participant_info->leave_middleware_default_qos) { + reader_qos.endpoint().history_memory_policy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + + reader_qos.data_sharing().off(); + } + + if (!get_datareader_qos(*qos_policies, reader_qos)) { + RMW_SET_ERROR_MSG("create_subscription() failed setting data reader QoS"); + return nullptr; + } + + info->listener_ = new (std::nothrow) SubListener(info, qos_policies->depth); + if (!info->listener_) { + RMW_SET_ERROR_MSG("create_subscriber() could not create subscriber listener"); + return nullptr; + } + + eprosima::fastdds::dds::DataReaderQos original_qos = reader_qos; + switch (subscription_options->require_unique_network_flow_endpoints) { + default: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_SYSTEM_DEFAULT: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_NOT_REQUIRED: + // Unique network flow endpoints not required. We leave the decission to the XML profile. + break; + + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED: + case RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED: + // Ensure we request unique network flow endpoints + if (nullptr == + PropertyPolicyHelper::find_property( + reader_qos.properties(), + "fastdds.unique_network_flows")) + { + reader_qos.properties().properties().emplace_back("fastdds.unique_network_flows", ""); + } + break; + } + + // Creates DataReader (with subscriber name to not change name policy) + info->data_reader_ = subscriber->create_datareader( + des_topic, + reader_qos, + info->listener_); + if (!info->data_reader_ && + (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == + subscription_options->require_unique_network_flow_endpoints)) + { + info->data_reader_ = subscriber->create_datareader( + des_topic, + original_qos, + info->listener_); + } + + if (!info->data_reader_) { + RMW_SET_ERROR_MSG("create_subscription() could not create data reader"); + return nullptr; + } + + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->data_reader_); + }); + + ///// + // Create RMW GID + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->data_reader_->guid()); + + rmw_subscription_t * rmw_subscription = rmw_subscription_allocate(); + if (!rmw_subscription) { + RMW_SET_ERROR_MSG("create_subscription() failed to allocate subscription"); + return nullptr; + } + auto cleanup_rmw_subscription = rcpputils::make_scope_exit( + [rmw_subscription]() { + rmw_free(const_cast(rmw_subscription->topic_name)); + rmw_subscription_free(rmw_subscription); + }); + 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( + "create_subscription() failed to allocate memory for subscription topic name"); + return nullptr; + } + memcpy(const_cast(rmw_subscription->topic_name), topic_name, strlen(topic_name) + 1); + + rmw_subscription->options = *subscription_options; + rmw_fastrtps_shared_cpp::__init_subscription_for_loans(rmw_subscription); + // TODO(iuhilnehc-ynos): update after rmw_fastrtps_cpp is confirmed + rmw_subscription->is_cft_enabled = false; + + topic.should_be_deleted = false; + cleanup_rmw_subscription.cancel(); + cleanup_datareader.cancel(); + return_type_support.cancel(); + cleanup_info.cancel(); + return rmw_subscription; +} +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/src/type_support_common.cpp b/src/type_support_common.cpp new file mode 100644 index 0000000..d602069 --- /dev/null +++ b/src/type_support_common.cpp @@ -0,0 +1,36 @@ +// 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/error_handling.h" + +#include "rmw_fastrtps_dynamic_cpp/TypeSupport.hpp" + +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" + +#include "rosidl_typesupport_introspection_c/identifier.h" + +#include "type_support_common.hpp" + +bool +using_introspection_c_typesupport(const char * typesupport_identifier) +{ + return typesupport_identifier == rosidl_typesupport_introspection_c__identifier; +} + +bool +using_introspection_cpp_typesupport(const char * typesupport_identifier) +{ + return typesupport_identifier == + rosidl_typesupport_introspection_cpp::typesupport_identifier; +} diff --git a/src/type_support_common.hpp b/src/type_support_common.hpp new file mode 100644 index 0000000..1846b7b --- /dev/null +++ b/src/type_support_common.hpp @@ -0,0 +1,112 @@ +// 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. + +#ifndef TYPE_SUPPORT_COMMON_HPP_ +#define TYPE_SUPPORT_COMMON_HPP_ + +#include +#include + +#include "rcpputils/find_and_replace.hpp" + +#include "rmw/error_handling.h" + +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +#include "rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp" +#include "rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp" + +#include "rosidl_typesupport_introspection_c/visibility_control.h" + +#include "rmw_fastrtps_dynamic_cpp/identifier.hpp" + +using MessageTypeSupport_c = rmw_fastrtps_dynamic_cpp::MessageTypeSupport< + rosidl_typesupport_introspection_c__MessageMembers +>; +using MessageTypeSupport_cpp = rmw_fastrtps_dynamic_cpp::MessageTypeSupport< + rosidl_typesupport_introspection_cpp::MessageMembers +>; +using TypeSupport_c = rmw_fastrtps_dynamic_cpp::TypeSupport< + rosidl_typesupport_introspection_c__MessageMembers +>; +using TypeSupport_cpp = rmw_fastrtps_dynamic_cpp::TypeSupport< + rosidl_typesupport_introspection_cpp::MessageMembers +>; + +using RequestTypeSupport_c = rmw_fastrtps_dynamic_cpp::RequestTypeSupport< + rosidl_typesupport_introspection_c__ServiceMembers, + rosidl_typesupport_introspection_c__MessageMembers +>; +using RequestTypeSupport_cpp = rmw_fastrtps_dynamic_cpp::RequestTypeSupport< + rosidl_typesupport_introspection_cpp::ServiceMembers, + rosidl_typesupport_introspection_cpp::MessageMembers +>; + +using ResponseTypeSupport_c = rmw_fastrtps_dynamic_cpp::ResponseTypeSupport< + rosidl_typesupport_introspection_c__ServiceMembers, + rosidl_typesupport_introspection_c__MessageMembers +>; +using ResponseTypeSupport_cpp = rmw_fastrtps_dynamic_cpp::ResponseTypeSupport< + rosidl_typesupport_introspection_cpp::ServiceMembers, + rosidl_typesupport_introspection_cpp::MessageMembers +>; + +bool +using_introspection_c_typesupport(const char * typesupport_identifier); + +bool +using_introspection_cpp_typesupport(const char * typesupport_identifier); + +template +ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_LOCAL +inline std::string +_create_type_name( + const void * untyped_members) +{ + auto members = static_cast(untyped_members); + if (!members) { + RMW_SET_ERROR_MSG("members handle is null"); + return ""; + } + + std::ostringstream ss; + std::string message_namespace(members->message_namespace_); + // Find and replace C namespace separator with C++, in case this is using C typesupport + message_namespace = rcpputils::find_and_replace(message_namespace, "__", "::"); + std::string message_name(members->message_name_); + if (!message_namespace.empty()) { + ss << message_namespace << "::"; + } + ss << "dds_::" << message_name << "_"; + return ss.str(); +} + +ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_LOCAL +inline std::string +_create_type_name( + const void * untyped_members, + const char * typesupport) +{ + if (using_introspection_c_typesupport(typesupport)) { + return _create_type_name( + untyped_members); + } else if (using_introspection_cpp_typesupport(typesupport)) { + return _create_type_name( + untyped_members); + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return ""; +} + +#endif // TYPE_SUPPORT_COMMON_HPP_ diff --git a/src/type_support_proxy.cpp b/src/type_support_proxy.cpp new file mode 100644 index 0000000..43c3b0a --- /dev/null +++ b/src/type_support_proxy.cpp @@ -0,0 +1,49 @@ +// 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_dynamic_cpp/TypeSupport.hpp" + +namespace rmw_fastrtps_dynamic_cpp +{ + +TypeSupportProxy::TypeSupportProxy(rmw_fastrtps_shared_cpp::TypeSupport * inner_type) +{ + setName(inner_type->getName()); + m_typeSize = inner_type->m_typeSize; + is_plain_ = inner_type->is_plain(); + max_size_bound_ = inner_type->is_bounded(); +} + +size_t TypeSupportProxy::getEstimatedSerializedSize( + const void * ros_message, const void * impl) const +{ + auto type_impl = static_cast(impl); + return type_impl->getEstimatedSerializedSize(ros_message, impl); +} + +bool TypeSupportProxy::serializeROSmessage( + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const +{ + auto type_impl = static_cast(impl); + return type_impl->serializeROSmessage(ros_message, ser, impl); +} + +bool TypeSupportProxy::deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const +{ + auto type_impl = static_cast(impl); + return type_impl->deserializeROSmessage(deser, ros_message, impl); +} + +} // namespace rmw_fastrtps_dynamic_cpp diff --git a/src/type_support_registry.cpp b/src/type_support_registry.cpp new file mode 100644 index 0000000..d7f635b --- /dev/null +++ b/src/type_support_registry.cpp @@ -0,0 +1,158 @@ +// Copyright 2020 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 "rcutils/logging_macros.h" + +#include "rmw/error_handling.h" + +#include "type_support_common.hpp" +#include "type_support_registry.hpp" + +template +type_support_ptr get_type_support( + const key_type & ros_type_support, map_type & map, creator fun) +{ + std::lock_guard guard(map.getMutex()); + RefCountedTypeSupport & item = map()[ros_type_support]; + if (0 == item.ref_count++) { + item.type_support = fun(); + if (!item.type_support) { + map().erase(ros_type_support); + return nullptr; + } + } + return item.type_support; +} + +template +void return_type_support( + const key_type & ros_type_support, map_type & map) +{ + std::lock_guard guard(map.getMutex()); + auto it = map().find(ros_type_support); + assert(it != map().end()); + if (0 == --it->second.ref_count) { + delete it->second.type_support; + map().erase(it); + } +} + +template +void cleanup(map_type & map, const char * msg) +{ + std::lock_guard guard(map.getMutex()); + if (!map().empty()) { + RCUTILS_LOG_DEBUG_NAMED( + "rmw_fastrtps_dynamic_cpp", + "TypeSupportRegistry %s is not empty. Cleaning it up...", msg); + for (auto it : map() ) { + delete it.second.type_support; + } + map().clear(); + } +} + +TypeSupportRegistry::~TypeSupportRegistry() +{ + cleanup(message_types_, "message_types_"); + cleanup(request_types_, "request_types_"); + cleanup(response_types_, "response_types_"); +} + +TypeSupportRegistry & TypeSupportRegistry::get_instance() +{ + static TypeSupportRegistry type_registry_instance; + return type_registry_instance; +} + +type_support_ptr TypeSupportRegistry::get_message_type_support( + const rosidl_message_type_support_t * ros_type_support) +{ + auto creator_fun = [&ros_type_support]() -> type_support_ptr + { + if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier)) { + auto members = static_cast( + ros_type_support->data); + return new MessageTypeSupport_c(members, ros_type_support); + } else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier)) { + auto members = static_cast( + ros_type_support->data); + return new MessageTypeSupport_cpp(members, ros_type_support); + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return nullptr; + }; + + return get_type_support(ros_type_support, message_types_, creator_fun); +} + +type_support_ptr TypeSupportRegistry::get_request_type_support( + const rosidl_service_type_support_t * ros_type_support) +{ + auto creator_fun = [&ros_type_support]() -> type_support_ptr + { + if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier)) { + auto members = static_cast( + ros_type_support->data); + return new RequestTypeSupport_c(members, ros_type_support); + } else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier)) { + auto members = static_cast( + ros_type_support->data); + return new RequestTypeSupport_cpp(members, ros_type_support); + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return nullptr; + }; + + return get_type_support(ros_type_support, request_types_, creator_fun); +} + +type_support_ptr TypeSupportRegistry::get_response_type_support( + const rosidl_service_type_support_t * ros_type_support) +{ + auto creator_fun = [&ros_type_support]() -> type_support_ptr + { + if (using_introspection_c_typesupport(ros_type_support->typesupport_identifier)) { + auto members = static_cast( + ros_type_support->data); + return new ResponseTypeSupport_c(members, ros_type_support); + } else if (using_introspection_cpp_typesupport(ros_type_support->typesupport_identifier)) { + auto members = static_cast( + ros_type_support->data); + return new ResponseTypeSupport_cpp(members, ros_type_support); + } + RMW_SET_ERROR_MSG("Unknown typesupport identifier"); + return nullptr; + }; + + return get_type_support(ros_type_support, response_types_, creator_fun); +} + +void TypeSupportRegistry::return_message_type_support( + const rosidl_message_type_support_t * ros_type_support) +{ + return_type_support(ros_type_support, message_types_); +} + +void TypeSupportRegistry::return_request_type_support( + const rosidl_service_type_support_t * ros_type_support) +{ + return_type_support(ros_type_support, request_types_); +} + +void TypeSupportRegistry::return_response_type_support( + const rosidl_service_type_support_t * ros_type_support) +{ + return_type_support(ros_type_support, response_types_); +} diff --git a/src/type_support_registry.hpp b/src/type_support_registry.hpp new file mode 100644 index 0000000..a3c6652 --- /dev/null +++ b/src/type_support_registry.hpp @@ -0,0 +1,74 @@ +// Copyright 2020 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. + +#ifndef TYPE_SUPPORT_REGISTRY_HPP_ +#define TYPE_SUPPORT_REGISTRY_HPP_ + +#include + +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/locked_object.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +#include "type_support_common.hpp" + +using type_support_ptr = rmw_fastrtps_dynamic_cpp::BaseTypeSupport *; + +/** + * A data structure to use as value type for the type registry. + */ +struct RefCountedTypeSupport +{ + type_support_ptr type_support = nullptr; + uint32_t ref_count = 0; +}; + +using msg_map_t = std::unordered_map; +using srv_map_t = std::unordered_map; + +class TypeSupportRegistry +{ +private: + LockedObject message_types_; + LockedObject request_types_; + LockedObject response_types_; + + TypeSupportRegistry() = default; + +public: + ~TypeSupportRegistry(); + + static TypeSupportRegistry & get_instance(); + + type_support_ptr get_message_type_support( + const rosidl_message_type_support_t * ros_type_support); + + type_support_ptr get_request_type_support( + const rosidl_service_type_support_t * ros_type_support); + + type_support_ptr get_response_type_support( + const rosidl_service_type_support_t * ros_type_support); + + void return_message_type_support( + const rosidl_message_type_support_t * ros_type_support); + + void return_request_type_support( + const rosidl_service_type_support_t * ros_type_support); + + void return_response_type_support( + const rosidl_service_type_support_t * ros_type_support); +}; + +#endif // TYPE_SUPPORT_REGISTRY_HPP_ diff --git a/test/test_get_native_entities.cpp b/test/test_get_native_entities.cpp new file mode 100644 index 0000000..89ac01a --- /dev/null +++ b/test/test_get_native_entities.cpp @@ -0,0 +1,173 @@ +// 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 "gtest/gtest.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" + +#include "rcutils/allocator.h" +#include "rcutils/strdup.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_dynamic_cpp/get_client.hpp" +#include "rmw_fastrtps_dynamic_cpp/get_participant.hpp" +#include "rmw_fastrtps_dynamic_cpp/get_publisher.hpp" +#include "rmw_fastrtps_dynamic_cpp/get_service.hpp" +#include "rmw_fastrtps_dynamic_cpp/get_subscriber.hpp" + +#include "test_msgs/msg/basic_types.h" +#include "test_msgs/srv/basic_types.h" + +class TestNativeEntities : public ::testing::Test +{ +protected: + void SetUp() override + { + rmw_init_options_t options = rmw_get_zero_initialized_init_options(); + rmw_ret_t ret = rmw_init_options_init(&options, rcutils_get_default_allocator()); + ASSERT_EQ(RMW_RET_OK, ret) << rcutils_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rmw_ret_t ret = rmw_init_options_fini(&options); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; + }); + options.enclave = rcutils_strdup("/", rcutils_get_default_allocator()); + ASSERT_STREQ("/", options.enclave); + ret = rmw_init(&options, &context); + ASSERT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; + constexpr char node_name[] = "my_node"; + constexpr char node_namespace[] = "/my_ns"; + node = rmw_create_node(&context, node_name, node_namespace); + ASSERT_NE(nullptr, node) << rmw_get_error_string().str; + } + + void TearDown() override + { + rmw_ret_t ret = rmw_destroy_node(node); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; + ret = rmw_shutdown(&context); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; + ret = rmw_context_fini(&context); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; + } + + rmw_context_t context{rmw_get_zero_initialized_context()}; + rmw_node_t * node{nullptr}; +}; + +TEST_F(TestNativeEntities, get_domain_participant) { + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_domain_participant(nullptr)); + + const char * implementation_identifier = node->implementation_identifier; + node->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_domain_participant(node)); + node->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_domain_participant(node)); +} + +TEST_F(TestNativeEntities, get_datawriter) { + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_datawriter(nullptr)); + + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "/test"; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + rmw_publisher_options_t options = rmw_get_default_publisher_options(); + rmw_publisher_t * pub = rmw_create_publisher(node, ts, topic_name, &qos_profile, &options); + ASSERT_NE(nullptr, pub) << rmw_get_error_string().str; + + const char * implementation_identifier = pub->implementation_identifier; + pub->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_datawriter(pub)); + pub->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_datawriter(pub)); + + rmw_ret_t ret = rmw_destroy_publisher(node, pub); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; +} + +TEST_F(TestNativeEntities, get_datareader) { + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_datareader(nullptr)); + + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "/test"; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + rmw_subscription_options_t options = rmw_get_default_subscription_options(); + rmw_subscription_t * sub = + rmw_create_subscription(node, ts, topic_name, &qos_profile, &options); + ASSERT_NE(nullptr, sub) << rmw_get_error_string().str; + + const char * implementation_identifier = sub->implementation_identifier; + sub->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_datareader(sub)); + sub->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_datareader(sub)); + + rmw_ret_t ret = rmw_destroy_subscription(node, sub); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; +} + +TEST_F(TestNativeEntities, get_service) { + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datareader(nullptr)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datawriter(nullptr)); + + const rosidl_service_type_support_t * ts = + ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + constexpr char service_name[] = "/test"; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + rmw_service_t * srv = rmw_create_service(node, ts, service_name, &qos_profile); + ASSERT_NE(nullptr, srv) << rmw_get_error_string().str; + + const char * implementation_identifier = srv->implementation_identifier; + srv->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datareader(srv)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datawriter(srv)); + srv->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datareader(srv)); + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datawriter(srv)); + + rmw_ret_t ret = rmw_destroy_service(node, srv); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; +} + +TEST_F(TestNativeEntities, get_client) { + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datawriter(nullptr)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datareader(nullptr)); + + const rosidl_service_type_support_t * ts = + ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + constexpr char service_name[] = "/test"; + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + rmw_client_t * client = rmw_create_client(node, ts, service_name, &qos_profile); + ASSERT_NE(nullptr, client) << rmw_get_error_string().str; + + const char * implementation_identifier = client->implementation_identifier; + client->implementation_identifier = "not-an-rmw-implementation-identifier"; + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datawriter(client)); + EXPECT_EQ(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datareader(client)); + client->implementation_identifier = implementation_identifier; + + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_request_datawriter(client)); + EXPECT_NE(nullptr, rmw_fastrtps_dynamic_cpp::get_response_datareader(client)); + + rmw_ret_t ret = rmw_destroy_client(node, client); + EXPECT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; +} diff --git a/test/test_logging.cpp b/test/test_logging.cpp new file mode 100644 index 0000000..f0ed403 --- /dev/null +++ b/test/test_logging.cpp @@ -0,0 +1,47 @@ +// 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 "gtest/gtest.h" + +#include "fastdds/dds/log/Log.hpp" + +#include "rmw/rmw.h" +#include "rmw/error_handling.h" + +TEST(TestLogging, rmw_logging) +{ + using Log = eprosima::fastdds::dds::Log; + + rmw_ret_t ret = rmw_set_log_severity(RMW_LOG_SEVERITY_DEBUG); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); + ret = rmw_set_log_severity(RMW_LOG_SEVERITY_INFO); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); + ret = rmw_set_log_severity(RMW_LOG_SEVERITY_WARN); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(Log::Kind::Warning, Log::GetVerbosity()); + ret = rmw_set_log_severity(RMW_LOG_SEVERITY_ERROR); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(Log::Kind::Error, Log::GetVerbosity()); + ret = rmw_set_log_severity(RMW_LOG_SEVERITY_FATAL); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(Log::Kind::Error, Log::GetVerbosity()); +} + +TEST(TestLogging, rmw_logging_bad_verbosity) +{ + rmw_ret_t ret = rmw_set_log_severity(static_cast(RMW_LOG_SEVERITY_FATAL + 1)); + EXPECT_EQ(ret, RMW_RET_ERROR); +}