From 19d777bf694dd870cd6d2ceeeb1f811e82a6aaf5 Mon Sep 17 00:00:00 2001 From: Timon Engelke Date: Sat, 21 May 2022 21:01:59 +0200 Subject: [PATCH] Import Upstream version 6.2.1 --- CHANGELOG.rst | 358 +++++++++++ CMakeLists.txt | 157 +++++ Doxyfile | 30 + QUALITY_DECLARATION.md | 213 +++++++ README.md | 7 + .../rmw_fastrtps_shared_cpp/TypeSupport.hpp | 118 ++++ .../create_rmw_gid.hpp | 33 + .../custom_client_info.hpp | 273 ++++++++ .../custom_event_info.hpp | 108 ++++ .../custom_participant_info.hpp | 177 ++++++ .../custom_publisher_info.hpp | 163 +++++ .../custom_service_info.hpp | 346 ++++++++++ .../custom_subscriber_info.hpp | 268 ++++++++ .../rmw_fastrtps_shared_cpp/guid_utils.hpp | 91 +++ .../init_rmw_context_impl.hpp | 36 ++ .../listener_thread.hpp | 34 + .../rmw_fastrtps_shared_cpp/locked_object.hpp | 49 ++ include/rmw_fastrtps_shared_cpp/names.hpp | 71 +++ .../namespace_prefix.hpp | 57 ++ .../rmw_fastrtps_shared_cpp/participant.hpp | 50 ++ include/rmw_fastrtps_shared_cpp/publisher.hpp | 34 + include/rmw_fastrtps_shared_cpp/qos.hpp | 207 ++++++ .../rmw_fastrtps_shared_cpp/rmw_common.hpp | 538 ++++++++++++++++ .../rmw_context_impl.hpp | 35 ++ include/rmw_fastrtps_shared_cpp/rmw_init.hpp | 43 ++ .../rmw_security_logging.hpp | 31 + .../rmw_fastrtps_shared_cpp/subscription.hpp | 40 ++ include/rmw_fastrtps_shared_cpp/utils.hpp | 189 ++++++ .../visibility_control.h | 56 ++ package.xml | 47 ++ rmw_fastrtps_shared_cpp-extras.cmake | 24 + src/TypeSupport_impl.cpp | 591 ++++++++++++++++++ src/create_rmw_gid.cpp | 36 ++ src/custom_publisher_info.cpp | 176 ++++++ src/custom_subscriber_info.cpp | 211 +++++++ src/demangle.cpp | 167 +++++ src/demangle.hpp | 57 ++ src/event_helpers.hpp | 33 + src/init_rmw_context_impl.cpp | 109 ++++ src/listener_thread.cpp | 180 ++++++ src/namespace_prefix.cpp | 69 ++ src/participant.cpp | 361 +++++++++++ src/publisher.cpp | 70 +++ src/qos.cpp | 214 +++++++ src/rmw_client.cpp | 164 +++++ src/rmw_compare_gids_equal.cpp | 52 ++ src/rmw_count.cpp | 100 +++ src/rmw_event.cpp | 111 ++++ src/rmw_features.cpp | 26 + src/rmw_get_endpoint_network_flow.cpp | 204 ++++++ src/rmw_get_gid_for_publisher.cpp | 43 ++ src/rmw_get_topic_endpoint_info.cpp | 126 ++++ src/rmw_guard_condition.cpp | 49 ++ src/rmw_init.cpp | 112 ++++ src/rmw_logging.cpp | 57 ++ src/rmw_node.cpp | 164 +++++ src/rmw_node_info_and_types.cpp | 249 ++++++++ src/rmw_node_names.cpp | 98 +++ src/rmw_publish.cpp | 145 +++++ src/rmw_publisher.cpp | 217 +++++++ src/rmw_qos.cpp | 36 ++ src/rmw_request.cpp | 121 ++++ src/rmw_response.cpp | 146 +++++ src/rmw_security_logging.cpp | 223 +++++++ src/rmw_service.cpp | 176 ++++++ src/rmw_service_names_and_types.cpp | 64 ++ src/rmw_service_server_is_available.cpp | 130 ++++ src/rmw_subscription.cpp | 275 ++++++++ src/rmw_take.cpp | 539 ++++++++++++++++ src/rmw_topic_names_and_types.cpp | 72 +++ src/rmw_trigger_guard_condition.cpp | 41 ++ src/rmw_wait.cpp | 251 ++++++++ src/rmw_wait_set.cpp | 106 ++++ src/subscription.cpp | 84 +++ src/time_utils.cpp | 35 ++ src/time_utils.hpp | 27 + src/types/custom_wait_set_info.hpp | 27 + src/types/event_types.hpp | 30 + src/types/guard_condition.hpp | 87 +++ src/utils.cpp | 212 +++++++ test/CMakeLists.txt | 55 ++ test/test_dds_attributes_to_rmw_qos.cpp | 160 +++++ test/test_dds_qos_to_rmw_qos.cpp | 161 +++++ test/test_guid_utils.cpp | 62 ++ test/test_logging.cpp | 50 ++ test/test_names.cpp | 92 +++ test/test_qos_profile_check_compatible.cpp | 101 +++ test/test_rmw_init_options.cpp | 242 +++++++ test/test_rmw_qos_to_dds_attributes.cpp | 247 ++++++++ test/test_security_logging.cpp | 275 ++++++++ 90 files changed, 12201 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_shared_cpp/TypeSupport.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/create_rmw_gid.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/custom_client_info.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/custom_event_info.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/custom_service_info.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/guid_utils.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/listener_thread.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/locked_object.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/names.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/namespace_prefix.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/participant.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/publisher.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/qos.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/rmw_common.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/rmw_context_impl.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/rmw_init.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/rmw_security_logging.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/subscription.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/utils.hpp create mode 100644 include/rmw_fastrtps_shared_cpp/visibility_control.h create mode 100644 package.xml create mode 100644 rmw_fastrtps_shared_cpp-extras.cmake create mode 100644 src/TypeSupport_impl.cpp create mode 100644 src/create_rmw_gid.cpp create mode 100644 src/custom_publisher_info.cpp create mode 100644 src/custom_subscriber_info.cpp create mode 100644 src/demangle.cpp create mode 100644 src/demangle.hpp create mode 100644 src/event_helpers.hpp create mode 100644 src/init_rmw_context_impl.cpp create mode 100644 src/listener_thread.cpp create mode 100644 src/namespace_prefix.cpp create mode 100644 src/participant.cpp create mode 100644 src/publisher.cpp create mode 100644 src/qos.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_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_security_logging.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/subscription.cpp create mode 100644 src/time_utils.cpp create mode 100644 src/time_utils.hpp create mode 100644 src/types/custom_wait_set_info.hpp create mode 100644 src/types/event_types.hpp create mode 100644 src/types/guard_condition.hpp create mode 100644 src/utils.cpp create mode 100644 test/CMakeLists.txt create mode 100644 test/test_dds_attributes_to_rmw_qos.cpp create mode 100644 test/test_dds_qos_to_rmw_qos.cpp create mode 100644 test/test_guid_utils.cpp create mode 100644 test/test_logging.cpp create mode 100644 test/test_names.cpp create mode 100644 test/test_qos_profile_check_compatible.cpp create mode 100644 test/test_rmw_init_options.cpp create mode 100644 test/test_rmw_qos_to_dds_attributes.cpp create mode 100644 test/test_security_logging.cpp diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..6227a77 --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,358 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rmw_fastrtps_shared_cpp +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +6.2.1 (2022-03-28) +------------------ +* Address linter waning for windows. (`#592 `_) +* Add pub/sub init, publish and take instrumentation using tracetools (`#591 `_) +* Add content filter topic feature (`#513 `_) +* Add sequence numbers to message info structure (`#587 `_) +* Contributors: Chen Lihui, Christophe Bedard, Ivan Santiago Paunovic, Tomoya Fujita + +6.2.0 (2022-03-01) +------------------ +* Add EventsExecutor (`#468 `_) +* Complete events support (`#583 `_) +* Install headers to include/${PROJECT_NAME} (`#578 `_) +* Change default to synchronous (`#571 `_) +* Contributors: Audrow Nash, Miguel Company, Shane Loretz, iRobot ROS + +6.1.2 (2022-01-14) +------------------ +* Fix cpplint error (`#574 `_) +* Contributors: Jacob Perron + +6.1.1 (2021-12-17) +------------------ +* Fixes for uncrustify 0.72 (`#572 `_) +* Contributors: Chris Lalancette + +6.1.0 (2021-11-19) +------------------ +* Add client/service QoS getters. (`#560 `_) +* Fix QoS depth settings for clients/service being ignored. (`#564 `_) +* Contributors: Chen Lihui, mauropasse + +6.0.0 (2021-09-15) +------------------ +* Update rmw_context_impl_t definition. (`#558 `_) +* Update the LoanManager to do internal locking. (`#552 `_) +* Contributors: Chris Lalancette, Michel Hidalgo + +5.2.2 (2021-08-09) +------------------ +* Pass the CRL down to Fast-DDS if available. (`#546 `_) +* Contributors: Chris Lalancette + +5.2.1 (2021-06-30) +------------------ +* Use the new rmw_dds_common::get_security_files (`#544 `_) +* Support for SubscriptionOptions::ignore_local_publications (`#536 `_) +* Change links from index.ros.org -> docs.ros.org (`#539 `_) +* Contributors: Chris Lalancette, Jose Antonio Moral + +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. +* Export rmw_dds_common as an rmw_fastrtps_shared_cpp dependency (`#530 `_) +* Update includes after rcutils/get_env.h deprecation (`#529 `_) +* Contributors: Christophe Bedard, Michel Hidalgo, 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) +------------------ +* Take and return new RMW_DURATION_INFINITE correctly (`#515 `_) +* Contributors: Emerson Knapp + +4.4.0 (2021-03-01) +------------------ +* Add RMW function to check QoS compatibility (`#511 `_) +* Capture cdr exceptions (`#505 `_) +* Contributors: Jacob Perron, Miguel Company + +4.3.0 (2021-01-25) +------------------ + +4.2.0 (2020-12-10) +------------------ +* Make sure to lock the mutex protecting client_endpoints\_. (`#492 `_) +* Contributors: Chris Lalancette + +4.1.0 (2020-12-08) +------------------ +* Use interface whitelist for localhost only (`#476 `_) +* Make use of error return value in decrement_context_impl_ref_count (`#488 `_) +* Remove unnecessary includes (`#487 `_) +* Use new time_utils function to limit rmw_time_t values to 32-bits (`#485 `_) +* New environment variable to change easily the publication mode (`#470 `_) +* Remove unused headers MessageTypeSupport.hpp and ServiceTypeSupport.hpp (`#481 `_) +* Contributors: Jacob Perron, José Luis Bueno López, Michael Jeronimo, Miguel Company, Stephen Brawner + +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 QD links for dependencies to rmw_fastrtps_shared_cpp QD. + * Provide external dependencies QD links. + * Update rmw_fastrtps_shared_cpp QD: Fast DDS + * Update README rmw_fastrtps_shared_cpp to QL2 +* Contributors: JLBuenoLopez-eProsima, Jaime Martin Losa, José Luis Bueno López, Michael Jeronimo + +3.1.4 (2020-10-02) +------------------ +* Perform fault injection in all creation/destruction APIs. (`#453 `_) +* Ensure rmw_destroy_node() completes despite run-time errors. (`#458 `_) +* Handle too large QoS queue depths. (`#457 `_) +* Update rmw_fastrtps_cpp and rmw_fastrtps_shared_cpp QDs to QL2. (`#456 `_) +* Contributors: Michel Hidalgo + +3.1.3 (2020-09-29) +------------------ +* checked client implementation and return RMW_RET_INCORRECT_RMW_IMPLEMENTATION (`#451 `_) +* Update service/client request/response API error returns (`#450 `_) +* Contributors: Alejandro Hernández Cordero, Jose Tomas Lorente + +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) +------------------ +* Add tests for RMW QoS to DDS attribute conversion. (`#449 `_) +* Make service/client construction/destruction implementation compliant (`#445 `_) +* Contributors: Michel Hidalgo + +3.1.0 (2020-09-23) +------------------ +* Inject faults on __rmw_publish() and run_listener_thread() call. (`#441 `_) +* Update gid API return codes. (`#440 `_) +* Update graph API return codes. (`#436 `_) +* Contributors: Michel Hidalgo + +3.0.0 (2020-09-18) +------------------ +* Update rmw_take_serialized() and rmw_take_with_message_info() error returns (`#435 `_) +* Update rmw_take() error returns (`#432 `_) +* Update rmw_publish() error returns (`#430 `_) +* Update rmw_publish_serialized_message() error returns (`#431 `_) +* Contributors: Jose Tomas Lorente, Lobotuerk + +2.6.0 (2020-08-28) +------------------ +* Improve __rmw_create_wait_set() implementation. (`#427 `_) +* Ensure compliant matched pub/sub count API. (`#424 `_) +* Ensure compliant publisher QoS queries. (`#425 `_) +* Fix memory leak that wait_set might be not destoryed in some case. (`#423 `_) +* Contributors: Chen Lihui, Michel Hidalgo + +2.5.0 (2020-08-07) +------------------ +* Avoid unused identifier variable warnings. (`#422 `_) +* Fix trying to get topic data that was already removed. (`#417 `_) +* Contributors: Chen Lihui, Michel Hidalgo + +2.4.0 (2020-08-06) +------------------ +* Ensure compliant subscription API. (`#419 `_) +* Use package path to TypeSupport.hpp headers in ServiceTypeSupport and MessageTypeSupport (`#415 `_) + Use package in path to TypeSupport header for ServiceTypeSupport/MessageTypeSupport +* Contributors: Jose Luis Rivero, 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) +------------------ +* Add missing thread-safety annotation in ServicePubListener (`#409 `_) +* Ensure compliant node construction/destruction API. (`#408 `_) +* Contributors: Michel Hidalgo + +2.0.0 (2020-07-08) +------------------ +* Update Quality Declarations to QL3. (`#404 `_) +* Contributors: Michel Hidalgo + +1.1.0 (2020-06-29) +------------------ +* Do not use string literals as implementation identifiers in tests. (`#402 `_) +* Ensure compliant init options API implementations. (`#399 `_) +* Finalize context iff shutdown. (`#396 `_) +* Handle RMW_DEFAULT_DOMAIN_ID. (`#394 `_) +* Make service wait for response reader (`#390 `_) +* Contributors: Michel Hidalgo, Miguel Company + +1.0.1 (2020-06-01) +------------------ +* Add Security Vulnerability Policy pointing to REP-2006 (`#389 `_) +* Do not compile assert death tests in Release builds (`#393 `_) +* Add test coverage for name mangling and namespacing-specific API (`#388 `_) +* Add test coverage for GUID utilities (`#387 `_) +* Drop unused TopicCache sources (`#386 `_) +* Add test coverage for rmw_init_options API (`#385 `_) +* Update QDs for 1.0 (`#383 `_) +* Contributors: Chris Lalancette, Michel Hidalgo, Stephen Brawner + +1.0.0 (2020-05-12) +------------------ +* Remove API related to manual by node liveliness. (`#379 `_) +* Update quality declarations on feature testing. (`#380 `_) +* Contributors: Ivan Santiago Paunovic, Michel Hidalgo + +0.9.1 (2020-05-08) +------------------ +* Fill service_info timestamps from sample_info (`#378 `_) +* Fix unused variabled warning (`#377 `_) +* Add basic support for security logging plugin (`#362 `_) +* Add package READMEs and QUALITY_DECLARATION files (`#375 `_) +* Added doxyfiles (`#372 `_) +* Contributors: Alejandro Hernández Cordero, Ingo Lütkebohle, Jacob Perron, Kyle Fazzari, brawner + +0.9.0 (2020-04-28) +------------------ +* Feature/services timestamps. (`#369 `_) +* Add support for taking a sequence of messages. (`#366 `_) +* Fill message_info timestamp. (`#368 `_) +* Export targets in a addition to include directories / libraries. (`#371 `_) +* Support for API break on Fast RTPS 2.0. (`#370 `_) +* security-context -> enclave. (`#365 `_) +* Switch to one Participant per Context. (`#312 `_) +* Correct error message when event is not supported. (`#358 `_) +* 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 `_) +* Change rmw_topic_endpoint_info_array.count to .size. (`#348 `_) +* Code style only: wrap after open parenthesis if not in one line. (`#347 `_) +* Fix unprotected use of mutex-guarded variable. (`#345 `_) +* Passing down type support information (`#342 `_) +* Implement functions to get publisher and subcription informations like QoS policies from topic name. (`#336 `_) +* Contributors: Dirk Thomas, Emerson Knapp, Ingo Lütkebohle, Ivan Santiago Paunovic, Jaison Titus, Miaofei Mei, Michael Carroll, Miguel Company, Mikael Arguedas + +0.8.1 (2019-10-23) +------------------ +* Restrict traffic to localhost only if env var is provided (`#331 `_) +* Added new functions which can be used to get rmw_qos_profile_t from WriterQos and ReaderQos (`#328 `_) +* Renamed dds_qos_to_rmw_qos to dds_attributes_to_rmw_qos (`#330 `_) +* Contributors: Brian Marchi, jaisontj + +0.8.0 (2019-09-25) +------------------ +* Correct error message (`#320 `_) +* Return specific error code when node is not found (`#311 `_) +* Correct linter failure (`#318 `_) +* Fix bug in graph API by node (`#316 `_) +* fix method name change from 1.8.1->1.9.0 (`#302 `_) +* Add missing lock guards for discovered_names and discovered_namespaces (`#301 `_) +* Add function for getting clients by node (`#293 `_) +* Enable manual_by_node and node liveliness assertion (`#298 `_) +* Enable assert liveliness on publisher. (`#296 `_) +* Use rcpputils::find_and_replace instead of std::regex_replace (`#291 `_) +* Fix a comparison with a sign mismatch (`#288 `_) +* Implement get_actual_qos() for subscriptions (`#287 `_) +* add missing qos setings in get_actual_qos() (`#284 `_) +* Fix ABBA deadlock. +* Contributors: Chris Lalancette, Emerson Knapp, Jacob Perron, M. M, Scott K Logan, William Woodall, ivanpauno + +0.7.3 (2019-05-29) +------------------ +* Protection of discovered_names and discovered_namespaces (`#283 `_) +* Disable all liveliness until it is actually supported (`#282 `_) +* Contributors: Emerson Knapp, MiguelCompany + +0.7.2 (2019-05-20) +------------------ +* fix log_debug typo in rmw_count (`#279 `_) +* Fastrtps18 event callbacks policies (`#275 `_) +* Centralize topic name creation logic and update to match FastRTPS 1.8 API (`#272 `_) +* Contributors: 1r0b1n0, Emerson Knapp, Nick Burek + +0.7.1 (2019-05-08) +------------------ +* Support arbitrary message namespaces (`#266 `_) +* Set more correct return values for unimplemented features (`#276 `_) +* Add qos interfaces with no-op (`#271 `_) +* Updates for preallocation API. (`#274 `_) +* Fix logging in rmw_node_info_and_types.cpp (`#273 `_) +* Contributors: Emerson Knapp, Jacob Perron, Michael Carroll, Ross Desmond, Thomas Moulard + +0.7.0 (2019-04-13) +------------------ +* Thread safety annotation - minimally intrusive first pass (`#259 `_) +* Add function to get publisher actual qos settings (`#267 `_) +* Fixed race condition between taking sample and updating counter. (`#264 `_) +* Fix cpplint error +* change count type to size_t to avoid warning (`#262 `_) +* update listener logic for accurate counting (`#262 `_) +* Make sure to include the C++ headers used by these headers. (`#256 `_) +* pass context to wait set and fini context (`#252 `_) +* Improve service_is_available logic to protect that client is waiting forever (`#238 `_) +* Merge pull request `#250 `_ from ros2/support_static_lib +* use namespace_prefix from shared package +* make namespace_prefix header public +* Use empty() to check for an empty string (`#247 `_) +* We can compare a std::string with a const char* using operator==, simplifies the code (`#248 `_) +* Use empty() instead of size() to check if a vector/map contains elements and fixed some incorrect logging (`#245 `_) +* Fix guard condition trigger error (`#235 `_) +* Contributors: Chris Lalancette, Dirk Thomas, DongheeYe, Emerson Knapp, Jacob Perron, Johnny Willemsen, Ricardo González, William Woodall, ivanpauno + +0.6.1 (2018-12-06) +------------------ +* Add topic cache object for managing topic relations (`#236 `_) +* Fix lint: remove trailing whitespace (`#244 `_) +* Fastrtps 1.7.0 (`#233 `_) +* RMW_FastRTPS configuration from XML only (`#243 `_) +* Methods to retrieve matched counts on pub/sub (`#234 `_) +* use uint8_array (`#240 `_) +* Contributors: Jacob Perron, Juan Carlos, Karsten Knese, Michael Carroll, MiguelCompany, Ross Desmond + +0.6.0 (2018-11-16) +------------------ +* use new error handling API from rcutils (`#231 `_) +* Add semicolons to all RCLCPP and RCUTILS macros. (`#229 `_) +* separating identity and permission CAs (`#227 `_) +* Include node namespaces in get_node_names (`#224 `_) +* allow builtin reader/writer to reallocate memory if needed (`#221 `_) +* Improve runtime performance of `rmw_count_XXX` functions (`#216 `_) (`#217 `_) +* Merge pull request `#218 `_ from ros2/pr203 +* Refs `#3061 `_. Leaving common code only on rmw_fastrtps_shared_cpp. +* Refs `#3061 `_. Package rmw_fastrtps_cpp copied to rmw_fastrtps_shared_cpp. +* Contributors: Chris Lalancette, Dirk Thomas, Guillaume Autran, Michael Carroll, Miguel Company, Mikael Arguedas, William Woodall + +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..988704e --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,157 @@ +# 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_shared_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(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wthread-safety) +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(rosidl_typesupport_introspection_c REQUIRED) +find_package(rosidl_typesupport_introspection_cpp REQUIRED) +find_package(tracetools 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) + +add_library(rmw_fastrtps_shared_cpp + src/custom_publisher_info.cpp + src/custom_subscriber_info.cpp + src/create_rmw_gid.cpp + src/demangle.cpp + src/init_rmw_context_impl.cpp + src/listener_thread.cpp + src/namespace_prefix.cpp + src/participant.cpp + src/publisher.cpp + src/qos.cpp + src/rmw_client.cpp + src/rmw_compare_gids_equal.cpp + src/rmw_count.cpp + src/rmw_event.cpp + src/rmw_features.cpp + src/rmw_get_endpoint_network_flow.cpp + src/rmw_get_gid_for_publisher.cpp + src/rmw_get_topic_endpoint_info.cpp + src/rmw_guard_condition.cpp + src/rmw_init.cpp + src/rmw_logging.cpp + src/rmw_node.cpp + src/rmw_node_info_and_types.cpp + 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_security_logging.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/subscription.cpp + src/time_utils.cpp + src/TypeSupport_impl.cpp + src/utils.cpp +) +target_include_directories(rmw_fastrtps_shared_cpp + PUBLIC + "$" + "$") +target_link_libraries(rmw_fastrtps_shared_cpp + fastcdr fastrtps +) + +# specific order: dependents before dependencies +ament_target_dependencies(rmw_fastrtps_shared_cpp + "rcpputils" + "rcutils" + "rmw" + "rmw_dds_common" + "rosidl_typesupport_introspection_c" + "rosidl_typesupport_introspection_cpp" + "tracetools" +) + +# 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_SHARED_CPP_BUILDING_LIBRARY") + +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(rmw_fastrtps_shared_cpp) + +# Export modern CMake targets +ament_export_targets(rmw_fastrtps_shared_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(rosidl_typesupport_introspection_c) +ament_export_dependencies(rosidl_typesupport_introspection_cpp) +ament_export_dependencies(tracetools) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + add_subdirectory(test) +endif() + +ament_package( + CONFIG_EXTRAS_POST "rmw_fastrtps_shared_cpp-extras.cmake" +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install( + TARGETS rmw_fastrtps_shared_cpp EXPORT rmw_fastrtps_shared_cpp + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) diff --git a/Doxyfile b/Doxyfile new file mode 100644 index 0000000..6bab21f --- /dev/null +++ b/Doxyfile @@ -0,0 +1,30 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "rmw_fastrtps_shared_cpp" +PROJECT_NUMBER = master +PROJECT_BRIEF = "Code shared on static and dynamic type support of rmw_fastrtps_cpp." + +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_SHARED_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/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +TAGFILES += "../../../../doxygen_tag_files/rcpputils.tag=http://docs.ros2.org/latest/api/rcpputils/" +# Uncomment to generate tag files for cross-project linking. +GENERATE_TAGFILE = "../../../../doxygen_tag_files/rmw_fastrtps_shared_cpp.tag" diff --git a/QUALITY_DECLARATION.md b/QUALITY_DECLARATION.md new file mode 100644 index 0000000..a910245 --- /dev/null +++ b/QUALITY_DECLARATION.md @@ -0,0 +1,213 @@ +This document is a declaration of software quality for the `rmw_fastrtps_shared_cpp` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). + +# `rmw_fastrtps_shared_cpp` Quality Declaration + +The package `rmw_fastrtps_shared_cpp` claims to be in the **Quality Level 2** category. + +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Requirements for Quality Level 2 in REP-2004](https://www.ros.org/reps/rep-2004.html). + +## Version Policy [1] + +### Version Scheme [1.i] + +`rmw_fastrtps_shared_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_shared_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_shared_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_shared_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_shared_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. + +### 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_shared_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_shared_cpp` has embedded API documentation. It is not yet hosted publicly. + +### License [3.iii] + +The license for `rmw_fastrtps_shared_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_release/lastBuild/testReport/rmw_fastrtps_shared_cpp/copyright/) + +### Copyright Statements [3.iv] + +The copyright holders each provide a statement of copyright in each source code file in `rmw_fastrtps_shared_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_release/lastBuild/testReport/rmw_fastrtps_shared_cpp/copyright/). + +## Testing [4] + +### Feature Testing [4.i] + +All `rmw_fastrtps_shared_cpp` public features support ROS middleware feature implementations in `rmw_fastrtps_cpp` and `rmw_fastrtps_dynamic_cpp`. + +Unit, integration, and system tests for ROS middlewares 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 coverage. + +### Public API Testing [4.ii] + +All `rmw_fastrtps_shared_cpp` public API supports API implementations in in `rmw_fastrtps_cpp` and `rmw_fastrtps_dynamic_cpp`. + +Unit tests for many parts of this API provide coverage, and they are located in the [`test`](https://github.com/ros2/rmw_fastrtps/tree/master/rmw_fastrtps_shared_cpp/test) directory. +New additions or changes to this API require tests before being added. + +Unit, integration, and system tests for `rmw_fastrtps_cpp` and `rmw_fastrtps_dynamic_cpp` 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, further extend coverage. + +### Coverage [4.iii] + +`rmw_fastrtps_shared_cpp` follows the recommendations for ROS Core packages in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#code-coverage), and opts to use branch coverage instead of line coverage. + +This includes: + +- tracking and reporting line coverage statistics +- achieving and maintaining a reasonable branch line coverage (90-100%) +- no lines are manually skipped in coverage calculations + +Changes are required to make a best effort to keep or increase coverage before being accepted, but decreases are allowed if properly justified and accepted by reviewers. + +Current coverage statistics can be viewed +[here](https://ci.ros2.org/job/ci_linux_coverage/lastSuccessfulBuild/cobertura/install_rmw_fastrtps_shared_cpp_include_rmw_fastrtps_shared_cpp/), +[here](https://ci.ros2.org/job/ci_linux_coverage/lastSuccessfulBuild/cobertura/src_ros2_rmw_fastrtps_rmw_fastrtps_shared_cpp_include_rmw_fastrtps_shared_cpp/), +[here](https://ci.ros2.org/job/ci_linux_coverage/lastSuccessfulBuild/cobertura/src_ros2_rmw_fastrtps_rmw_fastrtps_shared_cpp_src/), and +[here](https://ci.ros2.org/job/ci_linux_coverage/lastSuccessfulBuild/cobertura/src_ros2_rmw_fastrtps_rmw_fastrtps_shared_cpp_src_types/). + +This package claims to meet the coverage requirements for the current quality level, even though it doesn't have 95% line coverage. +The justification is that the only uncovered lines have to do with system resource exhaustion and Fast DDS internal failure. + +A summary of how these statistics are calculated can be found in the [ROS 2 On-boarding guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#note-on-coverage-runs). + +### Performance [4.iv] + +`rmw_fastrtps_shared_cpp` does not currently have performance tests. + +### Linters and Static Analysis [4.v] + +`rmw_fastrtps_shared_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_release/1525/testReport/rmw_fastrtps_shared_cpp). + +## Dependencies [5] + +### Direct Runtime ROS Dependencies [5.i]/[5.ii] + +`rmw_fastrtps_shared_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) +* `rcutils`: [QUALITY DECLARATION](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md) +* `rcpputils`: [QUALITY DECLARATION](https://github.com/ros2/rcpputils/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) + +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_shared_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_shared_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_release/lastBuild/testReport/rmw_fastrtps_shared_cpp/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rmw_fastrtps_shared_cpp/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rmw_fastrtps_shared_cpp/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rmw_fastrtps_shared_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_shared_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..c0a2640 --- /dev/null +++ b/README.md @@ -0,0 +1,7 @@ +# `rmw_fastrtps_shared_cpp` + +`rmw_fastrtps_shared_cpp` provides common code for static and dynamic type support of rmw_fastrtps_cpp. + +## Quality Declaration + +This package claims to be in the **Quality Level 2** category, see the [Quality Declaration](QUALITY_DECLARATION.md) for more details. diff --git a/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp b/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp new file mode 100644 index 0000000..0a23d19 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/TypeSupport.hpp @@ -0,0 +1,118 @@ +// 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_SHARED_CPP__TYPESUPPORT_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__TYPESUPPORT_HPP_ + +#include +#include + +#include "fastdds/dds/topic/TopicDataType.hpp" + +#include "fastdds/rtps/common/InstanceHandle.h" +#include "fastdds/rtps/common/SerializedPayload.h" + +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + +#include "rcutils/logging_macros.h" + +#include "rosidl_runtime_c/message_type_support_struct.h" + +#include "./visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +// Publishers write method will receive a pointer to this struct +struct SerializedData +{ + bool is_cdr_buffer; // Whether next field is a pointer to a Cdr or to a plain ros message + void * data; + const void * impl; // RMW implementation specific data +}; + +class TypeSupport : public eprosima::fastdds::dds::TopicDataType +{ +public: + virtual size_t getEstimatedSerializedSize(const void * ros_message, const void * impl) const = 0; + + virtual bool serializeROSmessage( + const void * ros_message, eprosima::fastcdr::Cdr & ser, const void * impl) const = 0; + + virtual bool deserializeROSmessage( + eprosima::fastcdr::Cdr & deser, void * ros_message, const void * impl) const = 0; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool getKey( + void * data, + eprosima::fastrtps::rtps::InstanceHandle_t * ihandle, + bool force_md5 = false) override + { + (void)data; (void)ihandle; (void)force_md5; + return false; + } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool serialize(void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload) override; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool deserialize(eprosima::fastrtps::rtps::SerializedPayload_t * payload, void * data) override; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + std::function getSerializedSizeProvider(void * data) override; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void * createData() override; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void deleteData(void * data) override; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + inline bool is_bounded() const +#ifdef TOPIC_DATA_TYPE_API_HAS_IS_BOUNDED + override +#endif + { + return max_size_bound_; + } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + inline bool is_plain() const +#ifdef TOPIC_DATA_TYPE_API_HAS_IS_PLAIN + override +#endif + { + return is_plain_; + } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + virtual ~TypeSupport() {} + +protected: + RMW_FASTRTPS_SHARED_CPP_PUBLIC + TypeSupport(); + + bool max_size_bound_; + bool is_plain_; +}; + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool register_type_object( + const rosidl_message_type_support_t * type_supports, + const std::string & type_name); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__TYPESUPPORT_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/create_rmw_gid.hpp b/include/rmw_fastrtps_shared_cpp/create_rmw_gid.hpp new file mode 100644 index 0000000..aaecad5 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/create_rmw_gid.hpp @@ -0,0 +1,33 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__CREATE_RMW_GID_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__CREATE_RMW_GID_HPP_ + +#include "fastdds/rtps/common/Guid.h" + +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_gid_t +create_rmw_gid(const char * identifier, const eprosima::fastrtps::rtps::GUID_t & guid); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__CREATE_RMW_GID_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp b/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp new file mode 100644 index 0000000..9808170 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/custom_client_info.hpp @@ -0,0 +1,273 @@ +// 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_SHARED_CPP__CUSTOM_CLIENT_INFO_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__CUSTOM_CLIENT_INFO_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "fastcdr/FastBuffer.h" + +#include "fastdds/dds/core/status/PublicationMatchedStatus.hpp" +#include "fastdds/dds/core/status/SubscriptionMatchedStatus.hpp" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/publisher/DataWriterListener.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastdds/dds/subscriber/DataReaderListener.hpp" +#include "fastdds/dds/subscriber/SampleInfo.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" + +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/InstanceHandle.h" +#include "fastdds/rtps/common/SampleIdentity.h" + +#include "rcpputils/thread_safety_annotations.hpp" + +#include "rmw/event_callback_type.h" + +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +class ClientListener; +class ClientPubListener; + +typedef struct CustomClientInfo +{ + eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; + const void * request_type_support_impl_{nullptr}; + eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; + const void * response_type_support_impl_{nullptr}; + eprosima::fastdds::dds::DataReader * response_reader_{nullptr}; + eprosima::fastdds::dds::DataWriter * request_writer_{nullptr}; + + std::string request_topic_; + std::string response_topic_; + + ClientListener * listener_{nullptr}; + eprosima::fastrtps::rtps::GUID_t writer_guid_; + eprosima::fastrtps::rtps::GUID_t reader_guid_; + + const char * typesupport_identifier_{nullptr}; + ClientPubListener * pub_listener_{nullptr}; + std::atomic_size_t response_subscriber_matched_count_; + std::atomic_size_t request_publisher_matched_count_; +} CustomClientInfo; + +typedef struct CustomClientResponse +{ + eprosima::fastrtps::rtps::SampleIdentity sample_identity_; + std::unique_ptr buffer_; + eprosima::fastdds::dds::SampleInfo sample_info_ {}; +} CustomClientResponse; + +class ClientListener : public eprosima::fastdds::dds::DataReaderListener +{ +public: + explicit ClientListener(CustomClientInfo * info) + : info_(info), list_has_data_(false), + conditionMutex_(nullptr), conditionVariable_(nullptr) {} + + + void + on_data_available(eprosima::fastdds::dds::DataReader * reader) + { + assert(reader); + + CustomClientResponse response; + // Todo(sloretz) eliminate heap allocation pending eprosima/Fast-CDR#19 + response.buffer_.reset(new eprosima::fastcdr::FastBuffer()); + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = response.buffer_.get(); + data.impl = nullptr; // not used when is_cdr_buffer is true + if (reader->take_next_sample(&data, &response.sample_info_) == ReturnCode_t::RETCODE_OK) { + if (response.sample_info_.valid_data) { + response.sample_identity_ = response.sample_info_.related_sample_identity; + + if (response.sample_identity_.writer_guid() == info_->reader_guid_ || + response.sample_identity_.writer_guid() == info_->writer_guid_) + { + std::lock_guard lock(internalMutex_); + const eprosima::fastrtps::HistoryQosPolicy & history = reader->get_qos().history(); + if (eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == history.kind) { + while (list.size() >= static_cast(history.depth)) { + list.pop_front(); + } + } + + if (conditionMutex_ != nullptr) { + std::unique_lock clock(*conditionMutex_); + list.emplace_back(std::move(response)); + // the change to list_has_data_ needs to be mutually exclusive with + // rmw_wait() which checks hasData() and decides if wait() needs to + // be called + list_has_data_.store(true); + clock.unlock(); + conditionVariable_->notify_one(); + } else { + list.emplace_back(std::move(response)); + list_has_data_.store(true); + } + + std::unique_lock lock_mutex(on_new_response_m_); + + if (on_new_response_cb_) { + on_new_response_cb_(user_data_, 1); + } else { + unread_count_++; + } + } + } + } + } + + bool + getResponse(CustomClientResponse & response) + { + std::lock_guard lock(internalMutex_); + + if (conditionMutex_ != nullptr) { + std::unique_lock clock(*conditionMutex_); + return popResponse(response); + } + return popResponse(response); + } + + void + attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = conditionMutex; + conditionVariable_ = conditionVariable; + } + + void + detachCondition() + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = nullptr; + conditionVariable_ = nullptr; + } + + bool + hasData() + { + return list_has_data_.load(); + } + + void on_subscription_matched( + eprosima::fastdds::dds::DataReader * /* reader */, + const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final + { + if (info_ == nullptr) { + return; + } + if (info.current_count_change == 1) { + publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } else if (info.current_count_change == -1) { + publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } else { + return; + } + info_->response_subscriber_matched_count_.store(publishers_.size()); + } + + // Provide handlers to perform an action when a + // new event from this listener has ocurred + void + set_on_new_response_callback( + const void * user_data, + rmw_event_callback_t callback) + { + std::unique_lock lock_mutex(on_new_response_m_); + + if (callback) { + // Push events arrived before setting the the executor callback + if (unread_count_) { + callback(user_data, unread_count_); + unread_count_ = 0; + } + user_data_ = user_data; + on_new_response_cb_ = callback; + } else { + user_data_ = nullptr; + on_new_response_cb_ = nullptr; + } + } + +private: + bool popResponse(CustomClientResponse & response) RCPPUTILS_TSA_REQUIRES(internalMutex_) + { + if (!list.empty()) { + response = std::move(list.front()); + list.pop_front(); + list_has_data_.store(!list.empty()); + return true; + } + return false; + }; + + CustomClientInfo * info_; + std::mutex internalMutex_; + std::list list RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::atomic_bool list_has_data_; + std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::set publishers_; + + rmw_event_callback_t on_new_response_cb_{nullptr}; + const void * user_data_{nullptr}; + std::mutex on_new_response_m_; + uint64_t unread_count_ = 0; +}; + +class ClientPubListener : public eprosima::fastdds::dds::DataWriterListener +{ +public: + explicit ClientPubListener(CustomClientInfo * info) + : info_(info) + { + } + + void on_publication_matched( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus & info) final + { + if (info_ == nullptr) { + return; + } + if (info.current_count_change == 1) { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else if (info.current_count_change == -1) { + subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else { + return; + } + info_->request_publisher_matched_count_.store(subscriptions_.size()); + } + +private: + CustomClientInfo * info_; + std::set subscriptions_; +}; + +#endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_CLIENT_INFO_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp b/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp new file mode 100644 index 0000000..d7df466 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/custom_event_info.hpp @@ -0,0 +1,108 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__CUSTOM_EVENT_INFO_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__CUSTOM_EVENT_INFO_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "fastcdr/FastBuffer.h" + +#include "rmw/event.h" +#include "rmw/event_callback_type.h" + +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + + +class EventListenerInterface +{ +protected: + class ConditionalScopedLock; + +public: + /// Connect a condition variable so a waiter can be notified of new data. + virtual void attachCondition( + std::mutex * conditionMutex, + std::condition_variable * conditionVariable) = 0; + + /// Unset the information from attachCondition. + virtual void detachCondition() = 0; + + /// Check if there is new data available for a specific event type. + /** + * \param event_type The event type to check on. + * \return `true` if new data is available. + */ + virtual bool hasEvent(rmw_event_type_t event_type) const = 0; + + /// Take ready data for an event type. + /** + * \param event_type The event type to get data for. + * \param event_info A preallocated event information (from rmw/types.h) to fill with data + * \return `true` if data was successfully taken. + * \return `false` if data was not available, in this case nothing was written to event_info. + */ + virtual bool takeNextEvent(rmw_event_type_t event_type, void * event_info) = 0; + + // Provide handlers to perform an action when a + // new event from this listener has ocurred + virtual void set_on_new_event_callback( + const void * user_data, + rmw_event_callback_t callback) = 0; + + rmw_event_callback_t on_new_event_cb_{nullptr}; + const void * user_data_{nullptr}; + uint64_t unread_events_count_ = 0; + std::mutex on_new_event_m_; +}; + +class EventListenerInterface::ConditionalScopedLock +{ +public: + ConditionalScopedLock( + std::mutex * mutex, + std::condition_variable * condition_variable = nullptr) + : mutex_(mutex), cv_(condition_variable) + { + if (nullptr != mutex_) { + mutex_->lock(); + } + } + + ~ConditionalScopedLock() + { + if (nullptr != mutex_) { + mutex_->unlock(); + if (nullptr != cv_) { + cv_->notify_all(); + } + } + } + +private: + std::mutex * mutex_; + std::condition_variable * cv_; +}; + +struct CustomEventInfo +{ + virtual EventListenerInterface * getListener() const = 0; +}; + +#endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_EVENT_INFO_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp b/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp new file mode 100644 index 0000000..1ae23eb --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/custom_participant_info.hpp @@ -0,0 +1,177 @@ +// 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_SHARED_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ + +#include +#include +#include +#include + +#include "fastdds/dds/domain/DomainParticipant.hpp" +#include "fastdds/dds/domain/DomainParticipantListener.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" + +#include "fastdds/rtps/participant/ParticipantDiscoveryInfo.h" +#include "fastdds/rtps/reader/ReaderDiscoveryInfo.h" +#include "fastdds/rtps/writer/WriterDiscoveryInfo.h" + +#include "rcpputils/thread_safety_annotations.hpp" +#include "rcutils/logging_macros.h" + +#include "rmw/impl/cpp/key_value.hpp" +#include "rmw/qos_profiles.h" +#include "rmw/rmw.h" + +#include "rmw_dds_common/context.hpp" + +#include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +using rmw_dds_common::operator<<; + +class ParticipantListener; + +enum class publishing_mode_t +{ + ASYNCHRONOUS, // Asynchronous publishing mode + SYNCHRONOUS, // Synchronous publishing mode + AUTO // Use publishing mode set in XML file or Fast DDS default +}; + +typedef struct CustomParticipantInfo +{ + eprosima::fastdds::dds::DomainParticipant * participant_{nullptr}; + ParticipantListener * listener_{nullptr}; + + eprosima::fastdds::dds::Publisher * publisher_{nullptr}; + eprosima::fastdds::dds::Subscriber * subscriber_{nullptr}; + + // Protects creation and destruction of topics, readers and writers + mutable std::mutex entity_creation_mutex_; + + // Flag to establish if the QoS of the DomainParticipant, + // its DataWriters, and its DataReaders are going + // to be configured only from an XML file or if + // their settings are going to be overwritten by code + // with the default configuration. + bool leave_middleware_default_qos; + publishing_mode_t publishing_mode; +} CustomParticipantInfo; + +class ParticipantListener : public eprosima::fastdds::dds::DomainParticipantListener +{ +public: + explicit ParticipantListener( + const char * identifier, + rmw_dds_common::Context * context) + : context(context), + identifier_(identifier) + {} + + void on_participant_discovery( + eprosima::fastdds::dds::DomainParticipant *, + eprosima::fastrtps::rtps::ParticipantDiscoveryInfo && info) override + { + switch (info.status) { + case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DISCOVERED_PARTICIPANT: + { + auto map = rmw::impl::cpp::parse_key_value(info.info.m_userData); + auto name_found = map.find("enclave"); + + if (name_found == map.end()) { + return; + } + auto enclave = + std::string(name_found->second.begin(), name_found->second.end()); + + context->graph_cache.add_participant( + rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier_, info.info.m_guid), + enclave); + break; + } + case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::REMOVED_PARTICIPANT: + // fall through + case eprosima::fastrtps::rtps::ParticipantDiscoveryInfo::DROPPED_PARTICIPANT: + context->graph_cache.remove_participant( + rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier_, info.info.m_guid)); + break; + default: + return; + } + } + + void on_subscriber_discovery( + eprosima::fastdds::dds::DomainParticipant *, + eprosima::fastrtps::rtps::ReaderDiscoveryInfo && info) override + { + if (eprosima::fastrtps::rtps::ReaderDiscoveryInfo::CHANGED_QOS_READER != info.status) { + bool is_alive = + eprosima::fastrtps::rtps::ReaderDiscoveryInfo::DISCOVERED_READER == info.status; + process_discovery_info(info.info, is_alive, true); + } + } + + void on_publisher_discovery( + eprosima::fastdds::dds::DomainParticipant *, + eprosima::fastrtps::rtps::WriterDiscoveryInfo && info) override + { + if (eprosima::fastrtps::rtps::WriterDiscoveryInfo::CHANGED_QOS_WRITER != info.status) { + bool is_alive = + eprosima::fastrtps::rtps::WriterDiscoveryInfo::DISCOVERED_WRITER == info.status; + process_discovery_info(info.info, is_alive, false); + } + } + +private: + template + void + process_discovery_info(T & proxyData, bool is_alive, bool is_reader) + { + { + if (is_alive) { + rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown; + rtps_qos_to_rmw_qos(proxyData.m_qos, &qos_profile); + + context->graph_cache.add_entity( + rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier_, + proxyData.guid()), + proxyData.topicName().to_string(), + proxyData.typeName().to_string(), + rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier_, + iHandle2GUID(proxyData.RTPSParticipantKey())), + qos_profile, + is_reader); + } else { + context->graph_cache.remove_entity( + rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier_, + proxyData.guid()), + is_reader); + } + } + } + + rmw_dds_common::Context * context; + const char * const identifier_; +}; + +#endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_PARTICIPANT_INFO_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp b/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp new file mode 100644 index 0000000..37db4ca --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/custom_publisher_info.hpp @@ -0,0 +1,163 @@ +// 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_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ + +#include +#include +#include +#include + +#include "fastdds/dds/core/status/BaseStatus.hpp" +#include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" +#include "fastdds/dds/core/status/PublicationMatchedStatus.hpp" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/publisher/DataWriterListener.hpp" +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" + +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/InstanceHandle.h" + +#include "rcpputils/thread_safety_annotations.hpp" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/custom_event_info.hpp" + + +class PubListener; + +typedef struct CustomPublisherInfo : public CustomEventInfo +{ + virtual ~CustomPublisherInfo() = default; + + eprosima::fastdds::dds::DataWriter * data_writer_{nullptr}; + PubListener * listener_{nullptr}; + eprosima::fastdds::dds::TypeSupport type_support_; + const void * type_support_impl_{nullptr}; + rmw_gid_t publisher_gid{}; + const char * typesupport_identifier_{nullptr}; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + EventListenerInterface * + getListener() const final; +} CustomPublisherInfo; + +class PubListener : public EventListenerInterface, public eprosima::fastdds::dds::DataWriterListener +{ +public: + explicit PubListener(CustomPublisherInfo * info) + : deadline_changes_(false), + liveliness_changes_(false), + incompatible_qos_changes_(false), + conditionMutex_(nullptr), + conditionVariable_(nullptr) + { + (void) info; + } + + // DataWriterListener implementation + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_publication_matched( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus & info) final + { + std::lock_guard lock(internalMutex_); + if (info.current_count_change == 1) { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else if (info.current_count_change == -1) { + subscriptions_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } + } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_offered_deadline_missed( + eprosima::fastdds::dds::DataWriter * writer, + const eprosima::fastdds::dds::OfferedDeadlineMissedStatus & status) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_liveliness_lost( + eprosima::fastdds::dds::DataWriter * writer, + const eprosima::fastdds::dds::LivelinessLostStatus & status) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_offered_incompatible_qos( + eprosima::fastdds::dds::DataWriter *, + const eprosima::fastdds::dds::OfferedIncompatibleQosStatus &) final; + + // EventListenerInterface implementation + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool + hasEvent(rmw_event_type_t event_type) const final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void set_on_new_event_callback( + const void * user_data, + rmw_event_callback_t callback) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool + takeNextEvent(rmw_event_type_t event_type, void * event_info) final; + + // PubListener API + size_t subscriptionCount() + { + std::lock_guard lock(internalMutex_); + return subscriptions_.size(); + } + + void + attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = conditionMutex; + conditionVariable_ = conditionVariable; + } + + void + detachCondition() + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = nullptr; + conditionVariable_ = nullptr; + } + +private: + mutable std::mutex internalMutex_; + + std::set subscriptions_ + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + + std::atomic_bool deadline_changes_; + eprosima::fastdds::dds::OfferedDeadlineMissedStatus offered_deadline_missed_status_ + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + + std::atomic_bool liveliness_changes_; + eprosima::fastdds::dds::LivelinessLostStatus liveliness_lost_status_ + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + + std::atomic_bool incompatible_qos_changes_; + eprosima::fastdds::dds::OfferedIncompatibleQosStatus incompatible_qos_status_ + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + + std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); +}; + +#endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_PUBLISHER_INFO_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp b/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp new file mode 100644 index 0000000..0ddbfd0 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/custom_service_info.hpp @@ -0,0 +1,346 @@ +// 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_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "fastcdr/FastBuffer.h" + +#include "fastdds/dds/core/status/PublicationMatchedStatus.hpp" +#include "fastdds/dds/core/status/SubscriptionMatchedStatus.hpp" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/publisher/DataWriterListener.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastdds/dds/subscriber/DataReaderListener.hpp" +#include "fastdds/dds/subscriber/SampleInfo.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" + +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/InstanceHandle.h" +#include "fastdds/rtps/common/SampleIdentity.h" + +#include "rcpputils/thread_safety_annotations.hpp" + +#include "rmw/event_callback_type.h" + +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +class ServiceListener; +class ServicePubListener; + +enum class client_present_t +{ + FAILURE, // an error occurred when checking + MAYBE, // reader not matched, writer still present + YES, // reader matched + GONE // neither reader nor writer +}; + +typedef struct CustomServiceInfo +{ + eprosima::fastdds::dds::TypeSupport request_type_support_{nullptr}; + const void * request_type_support_impl_{nullptr}; + eprosima::fastdds::dds::TypeSupport response_type_support_{nullptr}; + const void * response_type_support_impl_{nullptr}; + eprosima::fastdds::dds::DataReader * request_reader_{nullptr}; + eprosima::fastdds::dds::DataWriter * response_writer_{nullptr}; + + ServiceListener * listener_{nullptr}; + ServicePubListener * pub_listener_{nullptr}; + + const char * typesupport_identifier_{nullptr}; +} CustomServiceInfo; + +typedef struct CustomServiceRequest +{ + eprosima::fastrtps::rtps::SampleIdentity sample_identity_; + eprosima::fastcdr::FastBuffer * buffer_; + eprosima::fastdds::dds::SampleInfo sample_info_ {}; + + CustomServiceRequest() + : buffer_(nullptr) {} +} CustomServiceRequest; + +class ServicePubListener : public eprosima::fastdds::dds::DataWriterListener +{ + using subscriptions_set_t = + std::unordered_set; + using clients_endpoints_map_t = + std::unordered_map; + +public: + explicit ServicePubListener(CustomServiceInfo * info) + { + (void) info; + } + + void + on_publication_matched( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::PublicationMatchedStatus & info) final + { + std::lock_guard lock(mutex_); + if (info.current_count_change == 1) { + subscriptions_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle)); + } else if (info.current_count_change == -1) { + eprosima::fastrtps::rtps::GUID_t erase_endpoint_guid = + eprosima::fastrtps::rtps::iHandle2GUID(info.last_subscription_handle); + subscriptions_.erase(erase_endpoint_guid); + auto endpoint = clients_endpoints_.find(erase_endpoint_guid); + if (endpoint != clients_endpoints_.end()) { + clients_endpoints_.erase(endpoint->second); + clients_endpoints_.erase(erase_endpoint_guid); + } + } else { + return; + } + cv_.notify_all(); + } + + template + bool + wait_for_subscription( + const eprosima::fastrtps::rtps::GUID_t & guid, + const std::chrono::duration & rel_time) + { + auto guid_is_present = [this, guid]() RCPPUTILS_TSA_REQUIRES(mutex_)->bool + { + return subscriptions_.find(guid) != subscriptions_.end(); + }; + + std::unique_lock lock(mutex_); + return cv_.wait_for(lock, rel_time, guid_is_present); + } + + client_present_t + check_for_subscription( + const eprosima::fastrtps::rtps::GUID_t & guid) + { + { + std::lock_guard lock(mutex_); + // Check if the guid is still in the map + if (clients_endpoints_.find(guid) == clients_endpoints_.end()) { + // Client is gone + return client_present_t::GONE; + } + } + // Wait for subscription + if (!wait_for_subscription(guid, std::chrono::milliseconds(100))) { + return client_present_t::MAYBE; + } + return client_present_t::YES; + } + + void endpoint_erase_if_exists(const eprosima::fastrtps::rtps::GUID_t & endpointGuid) + { + std::lock_guard lock(mutex_); + auto endpoint = clients_endpoints_.find(endpointGuid); + if (endpoint != clients_endpoints_.end()) { + clients_endpoints_.erase(endpoint->second); + clients_endpoints_.erase(endpointGuid); + } + } + + void endpoint_add_reader_and_writer( + const eprosima::fastrtps::rtps::GUID_t & readerGuid, + const eprosima::fastrtps::rtps::GUID_t & writerGuid) + { + std::lock_guard lock(mutex_); + clients_endpoints_.emplace(readerGuid, writerGuid); + clients_endpoints_.emplace(writerGuid, readerGuid); + } + +private: + std::mutex mutex_; + subscriptions_set_t subscriptions_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + clients_endpoints_map_t clients_endpoints_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + std::condition_variable cv_; +}; + +class ServiceListener : public eprosima::fastdds::dds::DataReaderListener +{ +public: + explicit ServiceListener(CustomServiceInfo * info) + : info_(info), list_has_data_(false), + conditionMutex_(nullptr), conditionVariable_(nullptr) + { + } + + void + on_subscription_matched( + eprosima::fastdds::dds::DataReader * /* reader */, + const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final + { + if (info.current_count_change == -1) { + info_->pub_listener_->endpoint_erase_if_exists( + eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } + } + + void + on_data_available(eprosima::fastdds::dds::DataReader * reader) final + { + assert(reader); + + CustomServiceRequest request; + request.buffer_ = new eprosima::fastcdr::FastBuffer(); + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = request.buffer_; + data.impl = nullptr; // not used when is_cdr_buffer is true + if (reader->take_next_sample(&data, &request.sample_info_) == ReturnCode_t::RETCODE_OK) { + if (request.sample_info_.valid_data) { + request.sample_identity_ = request.sample_info_.sample_identity; + // Use response subscriber guid (on related_sample_identity) when present. + const eprosima::fastrtps::rtps::GUID_t & reader_guid = + request.sample_info_.related_sample_identity.writer_guid(); + if (reader_guid != eprosima::fastrtps::rtps::GUID_t::unknown() ) { + request.sample_identity_.writer_guid() = reader_guid; + } + + // Save both guids in the clients_endpoints map + const eprosima::fastrtps::rtps::GUID_t & writer_guid = + request.sample_info_.sample_identity.writer_guid(); + info_->pub_listener_->endpoint_add_reader_and_writer(reader_guid, writer_guid); + + std::lock_guard lock(internalMutex_); + const eprosima::fastrtps::HistoryQosPolicy & history = reader->get_qos().history(); + if (eprosima::fastrtps::KEEP_LAST_HISTORY_QOS == history.kind) { + while (list.size() >= static_cast(history.depth)) { + list.pop_front(); + } + } + + if (conditionMutex_ != nullptr) { + std::unique_lock clock(*conditionMutex_); + list.push_back(request); + // the change to list_has_data_ needs to be mutually exclusive with + // rmw_wait() which checks hasData() and decides if wait() needs to + // be called + list_has_data_.store(true); + clock.unlock(); + conditionVariable_->notify_one(); + } else { + list.push_back(request); + list_has_data_.store(true); + } + + std::unique_lock lock_mutex(on_new_request_m_); + + if (on_new_request_cb_) { + on_new_request_cb_(user_data_, 1); + } else { + unread_count_++; + } + } + } + } + + CustomServiceRequest + getRequest() + { + std::lock_guard lock(internalMutex_); + CustomServiceRequest request; + + if (conditionMutex_ != nullptr) { + std::unique_lock clock(*conditionMutex_); + if (!list.empty()) { + request = list.front(); + list.pop_front(); + list_has_data_.store(!list.empty()); + } + } else { + if (!list.empty()) { + request = list.front(); + list.pop_front(); + list_has_data_.store(!list.empty()); + } + } + + return request; + } + + void + attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = conditionMutex; + conditionVariable_ = conditionVariable; + } + + void + detachCondition() + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = nullptr; + conditionVariable_ = nullptr; + } + + bool + hasData() + { + return list_has_data_.load(); + } + + // Provide handlers to perform an action when a + // new event from this listener has ocurred + void + set_on_new_request_callback( + const void * user_data, + rmw_event_callback_t callback) + { + std::unique_lock lock_mutex(on_new_request_m_); + + if (callback) { + // Push events arrived before setting the the executor callback + if (unread_count_) { + callback(user_data, unread_count_); + unread_count_ = 0; + } + user_data_ = user_data; + on_new_request_cb_ = callback; + } else { + user_data_ = nullptr; + on_new_request_cb_ = nullptr; + } + } + +private: + CustomServiceInfo * info_; + std::mutex internalMutex_; + std::list list RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::atomic_bool list_has_data_; + std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + + rmw_event_callback_t on_new_request_cb_{nullptr}; + const void * user_data_{nullptr}; + std::mutex on_new_request_m_; + uint64_t unread_count_ = 0; +}; + +#endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SERVICE_INFO_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp b/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp new file mode 100644 index 0000000..a130f2c --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp @@ -0,0 +1,268 @@ +// 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_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" +#include "fastdds/dds/core/status/LivelinessChangedStatus.hpp" +#include "fastdds/dds/core/status/SubscriptionMatchedStatus.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastdds/dds/subscriber/DataReaderListener.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/ContentFilteredTopic.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" + +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/InstanceHandle.h" + +#include "rcpputils/thread_safety_annotations.hpp" + +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/event_callback_type.h" + +#include "rmw_dds_common/context.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_event_info.hpp" + + +class SubListener; + +namespace rmw_fastrtps_shared_cpp +{ +struct LoanManager; +} // namespace rmw_fastrtps_shared_cpp + +struct CustomSubscriberInfo : public CustomEventInfo +{ + virtual ~CustomSubscriberInfo() = default; + + eprosima::fastdds::dds::DataReader * data_reader_ {nullptr}; + SubListener * listener_{nullptr}; + eprosima::fastdds::dds::TypeSupport type_support_; + const void * type_support_impl_{nullptr}; + rmw_gid_t subscription_gid_{}; + const char * typesupport_identifier_{nullptr}; + std::shared_ptr loan_manager_; + + // for re-create or delete content filtered topic + const rmw_node_t * node_ {nullptr}; + rmw_dds_common::Context * common_context_ {nullptr}; + eprosima::fastdds::dds::DomainParticipant * dds_participant_ {nullptr}; + eprosima::fastdds::dds::Subscriber * subscriber_ {nullptr}; + std::string topic_name_mangled_; + eprosima::fastdds::dds::TopicDescription * topic_ {nullptr}; + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic_ {nullptr}; + eprosima::fastdds::dds::DataReaderQos datareader_qos_; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + EventListenerInterface * + getListener() const final; +}; + +class SubListener : public EventListenerInterface, public eprosima::fastdds::dds::DataReaderListener +{ +public: + explicit SubListener(CustomSubscriberInfo * info, size_t qos_depth) + : data_(false), + deadline_changes_(false), + liveliness_changes_(false), + sample_lost_changes_(false), + incompatible_qos_changes_(false), + conditionMutex_(nullptr), + conditionVariable_(nullptr) + { + qos_depth_ = (qos_depth > 0) ? qos_depth : std::numeric_limits::max(); + // Field is not used right now + (void)info; + } + + // DataReaderListener implementation + void + on_subscription_matched( + eprosima::fastdds::dds::DataReader * reader, + const eprosima::fastdds::dds::SubscriptionMatchedStatus & info) final + { + { + std::lock_guard lock(internalMutex_); + if (info.current_count_change == 1) { + publishers_.insert(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } else if (info.current_count_change == -1) { + publishers_.erase(eprosima::fastrtps::rtps::iHandle2GUID(info.last_publication_handle)); + } + } + update_has_data(reader); + } + + void + on_data_available(eprosima::fastdds::dds::DataReader * reader) final + { + update_has_data(reader); + + std::unique_lock lock_mutex(on_new_message_m_); + + if (on_new_message_cb_) { + on_new_message_cb_(user_data_, 1); + } else { + new_data_unread_count_++; + } + } + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_requested_deadline_missed( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastrtps::RequestedDeadlineMissedStatus &) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_liveliness_changed( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastrtps::LivelinessChangedStatus &) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_sample_lost( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::SampleLostStatus &) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void + on_requested_incompatible_qos( + eprosima::fastdds::dds::DataReader *, + const eprosima::fastdds::dds::RequestedIncompatibleQosStatus &) final; + + // EventListenerInterface implementation + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool + hasEvent(rmw_event_type_t event_type) const final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + void set_on_new_event_callback( + const void * user_data, + rmw_event_callback_t callback) final; + + RMW_FASTRTPS_SHARED_CPP_PUBLIC + bool + takeNextEvent(rmw_event_type_t event_type, void * event_info) final; + + // SubListener API + void + attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = conditionMutex; + conditionVariable_ = conditionVariable; + } + + void + detachCondition() + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = nullptr; + conditionVariable_ = nullptr; + } + + bool + hasData() const + { + return data_.load(std::memory_order_relaxed); + } + + void + update_has_data(eprosima::fastdds::dds::DataReader * reader) + { + // Make sure to call into Fast DDS before taking the lock to avoid an + // ABBA deadlock between internalMutex_ and mutexes inside of Fast DDS. + auto unread_count = reader->get_unread_count(); + bool has_data = unread_count > 0; + + std::lock_guard lock(internalMutex_); + ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + data_.store(has_data, std::memory_order_relaxed); + } + + size_t publisherCount() + { + std::lock_guard lock(internalMutex_); + return publishers_.size(); + } + + // Provide handlers to perform an action when a + // new event from this listener has ocurred + void + set_on_new_message_callback( + const void * user_data, + rmw_event_callback_t callback) + { + std::unique_lock lock_mutex(on_new_message_m_); + + if (callback) { + // Push events arrived before setting the executor's callback + if (new_data_unread_count_) { + auto unread_count = std::min(new_data_unread_count_, qos_depth_); + callback(user_data, unread_count); + new_data_unread_count_ = 0; + } + user_data_ = user_data; + on_new_message_cb_ = callback; + } else { + user_data_ = nullptr; + on_new_message_cb_ = nullptr; + } + } + +private: + mutable std::mutex internalMutex_; + + std::atomic_bool data_; + + std::atomic_bool deadline_changes_; + eprosima::fastdds::dds::RequestedDeadlineMissedStatus requested_deadline_missed_status_ + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + + std::atomic_bool liveliness_changes_; + eprosima::fastdds::dds::LivelinessChangedStatus liveliness_changed_status_ + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + + std::atomic_bool sample_lost_changes_; + eprosima::fastdds::dds::SampleLostStatus sample_lost_status_ + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + + std::atomic_bool incompatible_qos_changes_; + eprosima::fastdds::dds::RequestedIncompatibleQosStatus incompatible_qos_status_ + RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + + std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + + std::set publishers_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + + rmw_event_callback_t on_new_message_cb_{nullptr}; + std::mutex on_new_message_m_; + size_t qos_depth_; + size_t new_data_unread_count_ = 0; +}; + +#endif // RMW_FASTRTPS_SHARED_CPP__CUSTOM_SUBSCRIBER_INFO_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/guid_utils.hpp b/include/rmw_fastrtps_shared_cpp/guid_utils.hpp new file mode 100644 index 0000000..e645f95 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/guid_utils.hpp @@ -0,0 +1,91 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__GUID_UTILS_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__GUID_UTILS_HPP_ + +#include +#include +#include +#include + +#include "fastdds/rtps/common/Guid.h" + +namespace rmw_fastrtps_shared_cpp +{ + +template +void +copy_from_byte_array_to_fastrtps_guid( + const ByteT * guid_byte_array, + eprosima::fastrtps::rtps::GUID_t * guid) +{ + static_assert( + std::is_same::value || std::is_same::value, + "ByteT should be either int8_t or uint8_t"); + assert(guid_byte_array); + assert(guid); + constexpr auto prefix_size = sizeof(guid->guidPrefix.value); + memcpy(guid->guidPrefix.value, guid_byte_array, prefix_size); + memcpy(guid->entityId.value, &guid_byte_array[prefix_size], guid->entityId.size); +} + +template +void +copy_from_fastrtps_guid_to_byte_array( + const eprosima::fastrtps::rtps::GUID_t & guid, + ByteT * guid_byte_array) +{ + static_assert( + std::is_same::value || std::is_same::value, + "ByteT should be either int8_t or uint8_t"); + assert(guid_byte_array); + constexpr auto prefix_size = sizeof(guid.guidPrefix.value); + memcpy(guid_byte_array, &guid.guidPrefix, prefix_size); + memcpy(&guid_byte_array[prefix_size], &guid.entityId, guid.entityId.size); +} + +struct hash_fastrtps_guid +{ + std::size_t operator()(const eprosima::fastrtps::rtps::GUID_t & guid) const + { + union u_convert { + uint8_t plain_value[sizeof(guid)]; + uint32_t plain_ints[sizeof(guid) / sizeof(uint32_t)]; + } u {}; + + static_assert( + sizeof(guid) == 16 && + sizeof(u.plain_value) == sizeof(u.plain_ints) && + offsetof(u_convert, plain_value) == offsetof(u_convert, plain_ints), + "Plain guid should be easily convertible to uint32_t[4]"); + + copy_from_fastrtps_guid_to_byte_array(guid, u.plain_value); + + constexpr std::size_t prime_1 = 7; + constexpr std::size_t prime_2 = 31; + constexpr std::size_t prime_3 = 59; + + size_t ret_val = prime_1 * u.plain_ints[0]; + ret_val = prime_2 * (u.plain_ints[1] + ret_val); + ret_val = prime_3 * (u.plain_ints[2] + ret_val); + ret_val = u.plain_ints[3] + ret_val; + + return ret_val; + } +}; + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__GUID_UTILS_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp b/include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp new file mode 100644 index 0000000..13ca78d --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp @@ -0,0 +1,36 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ + +#include "rmw/init.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +/// Increment `rmw_context_impl_t` reference count, destroying it if the count reaches zero. +/** + * Function that should be called when destroying a node. + */ +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +decrement_context_impl_ref_count(rmw_context_t * context); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__INIT_RMW_CONTEXT_IMPL_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/listener_thread.hpp b/include/rmw_fastrtps_shared_cpp/listener_thread.hpp new file mode 100644 index 0000000..a4537c4 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/listener_thread.hpp @@ -0,0 +1,34 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__LISTENER_THREAD_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__LISTENER_THREAD_HPP_ + +#include "rmw/init.h" + +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +run_listener_thread(rmw_context_t * context); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +join_listener_thread(rmw_context_t * context); + +} // namespace rmw_fastrtps_shared_cpp +#endif // RMW_FASTRTPS_SHARED_CPP__LISTENER_THREAD_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/locked_object.hpp b/include/rmw_fastrtps_shared_cpp/locked_object.hpp new file mode 100644 index 0000000..b880d04 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/locked_object.hpp @@ -0,0 +1,49 @@ +// Copyright 2018 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__LOCKED_OBJECT_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__LOCKED_OBJECT_HPP_ + +#include + +#include "rcpputils/thread_safety_annotations.hpp" + +template +class LockedObject +{ +private: + mutable std::mutex mutex_; + T object_ RCPPUTILS_TSA_GUARDED_BY(mutex_); + +public: + /** + * \return a reference to this object to lock. + */ + std::mutex & getMutex() const RCPPUTILS_TSA_RETURN_CAPABILITY(mutex_) + { + return mutex_; + } + + T & operator()() + { + return object_; + } + + const T & operator()() const + { + return object_; + } +}; + +#endif // RMW_FASTRTPS_SHARED_CPP__LOCKED_OBJECT_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/names.hpp b/include/rmw_fastrtps_shared_cpp/names.hpp new file mode 100644 index 0000000..b19e957 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/names.hpp @@ -0,0 +1,71 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__NAMES_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__NAMES_HPP_ + +#include + +#include "fastrtps/utils/fixed_size_string.hpp" +#include "rmw/types.h" +#include "namespace_prefix.hpp" + +/// Construct a topic name. +/** + * \param[in] prefix Required prefix for topic name. + * \param[in] base Required name of the topic. + * \param[in] suffix Optional suffix for topic name. + */ +inline +eprosima::fastrtps::string_255 +_mangle_topic_name( + const char * prefix, + const char * base, + const char * suffix = nullptr) +{ + std::ostringstream topicName; + if (prefix) { + topicName << prefix; + } + topicName << base; + if (suffix) { + topicName << suffix; + } + return topicName.str(); +} + +/// Construct a topic name according to proper conventions. +/** + * \param[in] qos_profile The QoS profile for the topic. + * \param[in] prefix Required prefix for topic name. + * \param[in] base Required name of the topic. + * \param[in] suffix Optional suffix for topic name. + */ +inline +eprosima::fastrtps::string_255 +_create_topic_name( + const rmw_qos_profile_t * qos_profile, + const char * prefix, + const char * base, + const char * suffix = nullptr) +{ + assert(qos_profile); + assert(base); + if (qos_profile->avoid_ros_namespace_conventions) { + prefix = nullptr; + } + return _mangle_topic_name(prefix, base, suffix); +} + +#endif // RMW_FASTRTPS_SHARED_CPP__NAMES_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/namespace_prefix.hpp b/include/rmw_fastrtps_shared_cpp/namespace_prefix.hpp new file mode 100644 index 0000000..a2063a0 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/namespace_prefix.hpp @@ -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. + +#ifndef RMW_FASTRTPS_SHARED_CPP__NAMESPACE_PREFIX_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__NAMESPACE_PREFIX_HPP_ + +#include +#include + +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +extern "C" +{ +RMW_FASTRTPS_SHARED_CPP_PUBLIC extern const char * const ros_topic_prefix; +RMW_FASTRTPS_SHARED_CPP_PUBLIC extern const char * const ros_service_requester_prefix; +RMW_FASTRTPS_SHARED_CPP_PUBLIC extern const char * const ros_service_response_prefix; + +RMW_FASTRTPS_SHARED_CPP_PUBLIC extern const std::vector _ros_prefixes; +} // extern "C" + +/// Returns `name` stripped of `prefix` if exists, if not return "". +/** + * \param[in] name string that will be stripped from prefix + * \param[in] prefix prefix to be stripped + * \return name stripped of prefix, or + * \return "" if name doesn't start with prefix + */ +RMW_FASTRTPS_SHARED_CPP_PUBLIC +std::string +_resolve_prefix(const std::string & name, const std::string & prefix); + +/// Return the ROS specific prefix if it exists, otherwise "". +RMW_FASTRTPS_SHARED_CPP_PUBLIC +std::string +_get_ros_prefix_if_exists(const std::string & topic_name); + +/// Returns the topic name stripped of and ROS specific prefix if exists. +RMW_FASTRTPS_SHARED_CPP_PUBLIC +std::string +_strip_ros_prefix_if_exists(const std::string & topic_name); + +/// Returns the list of ros prefixes +RMW_FASTRTPS_SHARED_CPP_PUBLIC +const std::vector & +_get_all_ros_prefixes(); +#endif // RMW_FASTRTPS_SHARED_CPP__NAMESPACE_PREFIX_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/participant.hpp b/include/rmw_fastrtps_shared_cpp/participant.hpp new file mode 100644 index 0000000..a123a30 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/participant.hpp @@ -0,0 +1,50 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__PARTICIPANT_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__PARTICIPANT_HPP_ + +#include "rmw/types.h" + +#include "rmw_dds_common/context.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +// This method will create a DDS DomainParticipant along with +// a DDS Publisher and a DDS Subscriber +// For the creation of DDS DataWriter see method create_publisher +// For the creation of DDS DataReader see method create_subscription +// Note that ROS 2 Publishers and Subscriptions correspond with DDS DataWriters +// and DataReaders respectively and not with DDS Publishers and Subscribers. +RMW_FASTRTPS_SHARED_CPP_PUBLIC +CustomParticipantInfo * +create_participant( + const char * identifier, + size_t domain_id, + const rmw_security_options_t * security_options, + bool localhost_only, + const char * enclave, + rmw_dds_common::Context * common_context); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +destroy_participant(CustomParticipantInfo * info); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__PARTICIPANT_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/publisher.hpp b/include/rmw_fastrtps_shared_cpp/publisher.hpp new file mode 100644 index 0000000..fa40362 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/publisher.hpp @@ -0,0 +1,34 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__PUBLISHER_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__PUBLISHER_HPP_ + +#include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +destroy_publisher( + const char * identifier, + CustomParticipantInfo * participant_info, + rmw_publisher_t * publisher); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__PUBLISHER_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/qos.hpp b/include/rmw_fastrtps_shared_cpp/qos.hpp new file mode 100644 index 0000000..ad57994 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/qos.hpp @@ -0,0 +1,207 @@ +// 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. + +#ifndef RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ + +#include +#include +#include +#include + +#include "fastrtps/qos/QosPolicies.h" + +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +is_valid_qos(const rmw_qos_profile_t & qos_policies); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +get_datareader_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastdds::dds::DataReaderQos & reader_qos); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +get_datawriter_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastdds::dds::DataWriterQos & writer_qos); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +get_topic_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastdds::dds::TopicQos & topic_qos); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_time_t +dds_duration_to_rmw(const eprosima::fastrtps::Duration_t & duration); + +/* + * Converts the DDS QOS Policy; of type DataWriterQos or DataReaderQos into rmw_qos_profile_t. + * + * \param[in] dds_qos of type DataWriterQos or DataReaderQos + * \param[out] qos the equivalent of the data in dds_qos as a rmw_qos_profile_t + */ +template +void +dds_qos_to_rmw_qos( + const DDSQoSPolicyT & dds_qos, + rmw_qos_profile_t * qos) +{ + switch (dds_qos.reliability().kind) { + case eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + break; + case eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + break; + default: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; + break; + } + + switch (dds_qos.durability().kind) { + case eprosima::fastdds::dds::TRANSIENT_LOCAL_DURABILITY_QOS: + qos->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + break; + case eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS: + qos->durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + break; + default: + qos->durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; + break; + } + + qos->deadline = dds_duration_to_rmw(dds_qos.deadline().period); + qos->lifespan = dds_duration_to_rmw(dds_qos.lifespan().duration); + + switch (dds_qos.liveliness().kind) { + case eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + break; + case eprosima::fastdds::dds::MANUAL_BY_TOPIC_LIVELINESS_QOS: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + break; + default: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; + break; + } + + qos->liveliness_lease_duration = dds_duration_to_rmw(dds_qos.liveliness().lease_duration); + + switch (dds_qos.history().kind) { + case eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS: + qos->history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + break; + case eprosima::fastdds::dds::KEEP_ALL_HISTORY_QOS: + qos->history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + break; + default: + qos->history = RMW_QOS_POLICY_HISTORY_UNKNOWN; + break; + } + qos->depth = static_cast(dds_qos.history().depth); +} + +/* + * Converts the RTPS QOS Policy; of type WriterQos or ReaderQos into rmw_qos_profile_t. + * Since WriterQos or ReaderQos do not have information about history and depth, + * these values are not set by this function. + * + * \param[in] rtps_qos of type WriterQos or ReaderQos + * \param[out] qos the equivalent of the data in rtps_qos as a rmw_qos_profile_t + */ +template +void +rtps_qos_to_rmw_qos( + const RTPSQoSPolicyT & rtps_qos, + rmw_qos_profile_t * qos) +{ + switch (rtps_qos.m_reliability.kind) { + case eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + break; + case eprosima::fastrtps::RELIABLE_RELIABILITY_QOS: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + break; + default: + qos->reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; + break; + } + + switch (rtps_qos.m_durability.kind) { + case eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS: + qos->durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + break; + case eprosima::fastrtps::VOLATILE_DURABILITY_QOS: + qos->durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + break; + default: + qos->durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; + break; + } + + qos->deadline = dds_duration_to_rmw(rtps_qos.m_deadline.period); + qos->lifespan = dds_duration_to_rmw(rtps_qos.m_lifespan.duration); + + switch (rtps_qos.m_liveliness.kind) { + case eprosima::fastrtps::AUTOMATIC_LIVELINESS_QOS: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + break; + case eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + break; + default: + qos->liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; + break; + } + qos->liveliness_lease_duration = dds_duration_to_rmw(rtps_qos.m_liveliness.lease_duration); +} + +extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC +void dds_qos_to_rmw_qos( + const eprosima::fastdds::dds::DataWriterQos & dds_qos, + rmw_qos_profile_t * qos); + +extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC +void dds_qos_to_rmw_qos( + const eprosima::fastdds::dds::DataReaderQos & dds_qos, + rmw_qos_profile_t * qos); + + +template +void +dds_attributes_to_rmw_qos( + const AttributeT & dds_qos, + rmw_qos_profile_t * qos); + +extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC +void +dds_attributes_to_rmw_qos( + const eprosima::fastrtps::PublisherAttributes & dds_qos, + rmw_qos_profile_t * qos); + +extern template RMW_FASTRTPS_SHARED_CPP_PUBLIC +void +dds_attributes_to_rmw_qos( + const eprosima::fastrtps::SubscriberAttributes & dds_qos, + rmw_qos_profile_t * qos); + +#endif // RMW_FASTRTPS_SHARED_CPP__QOS_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/rmw_common.hpp b/include/rmw_fastrtps_shared_cpp/rmw_common.hpp new file mode 100644 index 0000000..ede5410 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/rmw_common.hpp @@ -0,0 +1,538 @@ +// 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_SHARED_CPP__RMW_COMMON_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__RMW_COMMON_HPP_ + +#include "./visibility_control.h" + +#include "rmw/error_handling.h" +#include "rmw/event.h" +#include "rmw/features.h" +#include "rmw/rmw.h" +#include "rmw/topic_endpoint_info_array.h" +#include "rmw/types.h" +#include "rmw/names_and_types.h" +#include "rmw/network_flow_endpoint_array.h" + +namespace rmw_fastrtps_shared_cpp +{ + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_client( + const char * identifier, + rmw_node_t * node, + rmw_client_t * client); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_compare_gids_equal( + const char * identifier, + const rmw_gid_t * gid1, + const rmw_gid_t * gid2, + bool * result); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_count_publishers( + const char * identifier, + const rmw_node_t * node, + const char * topic_name, + size_t * count); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_count_subscribers( + const char * identifier, + const rmw_node_t * node, + const char * topic_name, + size_t * count); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_gid_for_publisher( + const char * identifier, + const rmw_publisher_t * publisher, + rmw_gid_t * gid); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_guard_condition_t * +__rmw_create_guard_condition(const char * identifier); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_trigger_guard_condition( + const char * identifier, + const rmw_guard_condition_t * guard_condition_handle); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_set_log_severity(rmw_log_severity_t severity); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_node_t * +__rmw_create_node( + rmw_context_t * context, + const char * identifier, + const char * name, + const char * namespace_); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_node( + const char * identifier, + rmw_node_t * node); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +const rmw_guard_condition_t * +__rmw_node_get_graph_guard_condition(const rmw_node_t * node); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_node_names( + const char * identifier, + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_init_event( + const char * identifier, + rmw_event_t * rmw_event, + const char * topic_endpoint_impl_identifier, + void * data, + rmw_event_type_t event_type); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_node_names_with_enclaves( + const char * identifier, + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * enclaves); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_publish( + const char * identifier, + const rmw_publisher_t * publisher, + const void * ros_message, + rmw_publisher_allocation_t * allocation); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_publish_serialized_message( + const char * identifier, + const rmw_publisher_t * publisher, + const rmw_serialized_message_t * serialized_message, + rmw_publisher_allocation_t * allocation); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_borrow_loaned_message( + const char * identifier, + const rmw_publisher_t * publisher, + const rosidl_message_type_support_t * type_support, + void ** ros_message); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_return_loaned_message_from_publisher( + const char * identifier, + const rmw_publisher_t * publisher, + void * loaned_message); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_publish_loaned_message( + const char * identifier, + const rmw_publisher_t * publisher, + const void * ros_message, + rmw_publisher_allocation_t * allocation); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_publisher_assert_liveliness( + const char * identifier, + const rmw_publisher_t * publisher); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_publisher_wait_for_all_acked( + const char * identifier, + const rmw_publisher_t * publisher, + rmw_time_t wait_timeout); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_publisher( + const char * identifier, + const rmw_node_t * node, + rmw_publisher_t * publisher); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_publisher_count_matched_subscriptions( + const rmw_publisher_t * publisher, + size_t * subscription_count); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_publisher_get_actual_qos( + const rmw_publisher_t * publisher, + rmw_qos_profile_t * qos); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_send_request( + const char * identifier, + const rmw_client_t * client, + const void * ros_request, + int64_t * sequence_id); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_request( + const char * identifier, + const rmw_service_t * service, + rmw_service_info_t * request_header, + void * ros_request, + bool * taken); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_response( + const char * identifier, + const rmw_client_t * client, + rmw_service_info_t * request_header, + void * ros_response, + bool * taken); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_send_response( + const char * identifier, + const rmw_service_t * service, + rmw_request_id_t * request_header, + void * ros_response); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_service( + const char * identifier, + rmw_node_t * node, + rmw_service_t * service); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_service_names_and_types( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + rmw_names_and_types_t * service_names_and_types); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_publisher_names_and_types_by_node( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_service_names_and_types_by_node( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rmw_names_and_types_t * service_names_and_types); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_client_names_and_types_by_node( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rmw_names_and_types_t * service_names_and_types); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_subscriber_names_and_types_by_node( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_service_server_is_available( + const char * identifier, + const rmw_node_t * node, + const rmw_client_t * client, + bool * is_available); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_subscription( + const char * identifier, + const rmw_node_t * node, + rmw_subscription_t * subscription, + bool reset_cft = false); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_subscription_count_matched_publishers( + const rmw_subscription_t * subscription, + size_t * publisher_count); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_subscription_get_actual_qos( + const rmw_subscription_t * subscription, + rmw_qos_profile_t * qos); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_subscription_set_content_filter( + rmw_subscription_t * subscription, + const rmw_subscription_content_filter_options_t * options); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_subscription_get_content_filter( + const rmw_subscription_t * subscription, + rcutils_allocator_t * allocator, + rmw_subscription_content_filter_options_t * options); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_service_response_publisher_get_actual_qos( + const rmw_service_t * service, + rmw_qos_profile_t * qos); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_service_request_subscription_get_actual_qos( + const rmw_service_t * service, + rmw_qos_profile_t * qos); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_client_request_publisher_get_actual_qos( + const rmw_client_t * client, + rmw_qos_profile_t * qos); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_client_response_subscription_get_actual_qos( + const rmw_client_t * client, + rmw_qos_profile_t * qos); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take( + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_subscription_allocation_t * allocation); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_sequence( + const char * identifier, + const rmw_subscription_t * subscription, + size_t count, + rmw_message_sequence_t * message_sequencxe, + rmw_message_info_sequence_t * message_info_sequence, + size_t * taken, + rmw_subscription_allocation_t * allocation); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_loaned_message_internal( + const char * identifier, + const rmw_subscription_t * subscription, + void ** loaned_message, + bool * taken, + rmw_message_info_t * message_info); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_return_loaned_message_from_subscription( + const char * identifier, + const rmw_subscription_t * subscription, + void * loaned_message); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_event( + const char * identifier, + const rmw_event_t * event_handle, + void * event_info, + bool * taken); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_with_info( + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_serialized_message( + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_subscription_allocation_t * allocation); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_take_serialized_message_with_info( + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_topic_names_and_types( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_wait( + const char * identifier, + 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); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_wait_set_t * +__rmw_create_wait_set(const char * identifier, rmw_context_t * context, size_t max_conditions); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_destroy_wait_set(const char * identifier, rmw_wait_set_t * wait_set); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_publishers_info_by_topic( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * publishers_info); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_get_subscriptions_info_by_topic( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * subscriptions_info); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +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); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +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); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +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); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_subscription_set_on_new_message_callback( + rmw_subscription_t * rmw_subscription, + rmw_event_callback_t callback, + const void * user_data); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_service_set_on_new_request_callback( + rmw_service_t * rmw_service, + rmw_event_callback_t callback, + const void * user_data); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_client_set_on_new_response_callback( + rmw_client_t * rmw_client, + rmw_event_callback_t callback, + const void * user_data); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +__rmw_event_set_callback( + rmw_event_t * rmw_event, + rmw_event_callback_t callback, + const void * user_data); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +__rmw_feature_supported(rmw_feature_t feature); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__RMW_COMMON_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/rmw_context_impl.hpp b/include/rmw_fastrtps_shared_cpp/rmw_context_impl.hpp new file mode 100644 index 0000000..3d8fad7 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/rmw_context_impl.hpp @@ -0,0 +1,35 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__RMW_CONTEXT_IMPL_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__RMW_CONTEXT_IMPL_HPP_ + +#include + +// Definition of struct rmw_context_impl_s as declared in rmw/init.h +struct rmw_context_impl_s +{ + /// Pointer to `rmw_dds_common::Context`. + void * common; + /// Pointer to `rmw_fastrtps_shared_cpp::CustomParticipantInfo`. + void * participant_info; + /// Mutex used to protect initialization/destruction. + std::mutex mutex; + /// Reference count. + uint64_t count; + /// Shutdown flag. + bool is_shutdown; +}; + +#endif // RMW_FASTRTPS_SHARED_CPP__RMW_CONTEXT_IMPL_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/rmw_init.hpp b/include/rmw_fastrtps_shared_cpp/rmw_init.hpp new file mode 100644 index 0000000..ce878fa --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/rmw_init.hpp @@ -0,0 +1,43 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__RMW_INIT_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__RMW_INIT_HPP_ + +#include "rmw/init.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/init_options.h" + +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +rmw_init_options_init( + const char * identifier, rmw_init_options_t * init_options, rcutils_allocator_t allocator); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +rmw_init_options_copy( + const char * identifier, const rmw_init_options_t * src, rmw_init_options_t * dst); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +rmw_init_options_fini(const char * identifier, rmw_init_options_t * init_options); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__RMW_INIT_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/rmw_security_logging.hpp b/include/rmw_fastrtps_shared_cpp/rmw_security_logging.hpp new file mode 100644 index 0000000..611fa92 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/rmw_security_logging.hpp @@ -0,0 +1,31 @@ +// Copyright 2020 Canonical Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__RMW_SECURITY_LOGGING_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__RMW_SECURITY_LOGGING_HPP_ + +#include "fastdds/rtps/attributes/PropertyPolicy.h" + +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +/// Apply any requested security logging configuration to the policy. +/** + * \param policy policy to which security logging properties may be added. + * \returns false if the requested configuration could not be applied (rmw error will be set). + * \returns true if the requested configuration was applied (or no configuration was requested). + */ +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool apply_security_logging_configuration(eprosima::fastrtps::rtps::PropertyPolicy & policy); + +#endif // RMW_FASTRTPS_SHARED_CPP__RMW_SECURITY_LOGGING_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/subscription.hpp b/include/rmw_fastrtps_shared_cpp/subscription.hpp new file mode 100644 index 0000000..ddd2f07 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/subscription.hpp @@ -0,0 +1,40 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RMW_FASTRTPS_SHARED_CPP__SUBSCRIPTION_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__SUBSCRIPTION_HPP_ + +#include "rmw/rmw.h" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/visibility_control.h" + +namespace rmw_fastrtps_shared_cpp +{ + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +void +__init_subscription_for_loans( + rmw_subscription_t * subscription); + +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +destroy_subscription( + const char * identifier, + CustomParticipantInfo * participant_info, + rmw_subscription_t * subscription, + bool reset_cft = false); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__SUBSCRIPTION_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/utils.hpp b/include/rmw_fastrtps_shared_cpp/utils.hpp new file mode 100644 index 0000000..88b8e18 --- /dev/null +++ b/include/rmw_fastrtps_shared_cpp/utils.hpp @@ -0,0 +1,189 @@ +// Copyright 2021 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_SHARED_CPP__UTILS_HPP_ +#define RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ + +#include +#include + +#include "fastdds/dds/topic/TopicDescription.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" + +#include "fastrtps/types/TypesBase.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +#include "rmw/rmw.h" + + +namespace rmw_fastrtps_shared_cpp +{ + +/** +* Auxiliary struct to cleanup a topic created during entity creation. +* It is similar to a unique_ptr and its custom deleter at the same time. +* +* The creation process of an entity should be as follows: +* - find_and_check_topic_and_type is called +* - If the type is not found, it is created and registered +* - cast_or_create_topic is called on a stack-allocated TopicHolder +* - An early return will delete the topic if necessary +* - create_datawriter or create_datareader is called +* - Rest of the initialization process is performed +* - Before correctly returning the created entity, field should_be_deleted is set to false +* to avoid deletion of the topic +*/ +struct TopicHolder +{ + ~TopicHolder() + { + if (should_be_deleted) { + participant->delete_topic(topic); + } + } + + eprosima::fastdds::dds::DomainParticipant * participant = nullptr; + eprosima::fastdds::dds::TopicDescription * desc = nullptr; + eprosima::fastdds::dds::Topic * topic = nullptr; + bool should_be_deleted = false; +}; + +/** +* Convert a Fast DDS return code into the corresponding rmw_ret_t +* \param[in] code The Fast DDS return code to be translated +* \return the corresponding rmw_ret_t value +*/ +RMW_FASTRTPS_SHARED_CPP_PUBLIC +rmw_ret_t +cast_error_dds_to_rmw(eprosima::fastrtps::types::ReturnCode_t code); + +/** +* Auxiliary method to reuse or create a topic during the creation of an entity. +* +* \param[in] participant DomainParticipant where the topic will be created. +* \param[in] desc TopicDescription returned by find_and_check_topic_and_type. +* \param[in] topic_name Name of the topic. +* \param[in] type_name Name of the type. +* \param[in] topic_qos QoS with which to create the topic. +* \param[in] is_writer_topic Whether the resulting topic will be used on a DataWriter. +* \param[out] topic_holder Will hold the pointer to the topic along with the necessary +* information for its deletion. +* When is_writer_topic is true, topic_holder->topic can be +* directly used on a create_datawriter call. +* When is_writer_topic is false, topic_holder->desc can be +* directly used on a create_datareader call. +* +* \return true when the topic was reused (topic_holder->should_be_deleted will be false) +* \return true when the topic was created (topic_holder->should_be_deleted will be true) +* \return false when the topic could not be created +*/ +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +cast_or_create_topic( + eprosima::fastdds::dds::DomainParticipant * participant, + eprosima::fastdds::dds::TopicDescription * desc, + const std::string & topic_name, + const std::string & type_name, + const eprosima::fastdds::dds::TopicQos & topic_qos, + bool is_writer_topic, + TopicHolder * topic_holder); + +/** +* Tries to find already registered topic and type. +* +* \param[in] participant_info CustomParticipantInfo associated to the context. +* \param[in] topic_name Name of the topic for the entity being created. +* \param[in] type_name Name of the type for the entity being created. +* \param[out] returned_topic TopicDescription for topic_name +* \param[out] returned_type TypeSupport for type_name +* +* \return false if `topic_name` was previously created with a different type name. +* \return true when there is no such conflict. Returned topic and type may be null +* if they were not previously registered on the participant. +*/ +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +find_and_check_topic_and_type( + const CustomParticipantInfo * participant_info, + const std::string & topic_name, + const std::string & type_name, + eprosima::fastdds::dds::TopicDescription ** returned_topic, + eprosima::fastdds::dds::TypeSupport * returned_type); + +/** +* Performs removal of associated topic and type. +* +* \param[in] participant_info CustomParticipantInfo associated to the context. +* \param[in] topic Topic of the entity being deleted. +* \param[in] type TypeSupport of the entity being deleted. +*/ +RMW_FASTRTPS_SHARED_CPP_PUBLIC +void +remove_topic_and_type( + const CustomParticipantInfo * participant_info, + const eprosima::fastdds::dds::TopicDescription * topic, + const eprosima::fastdds::dds::TypeSupport & type); + +/** +* Create content filtered topic. +* +* \param[in] participant DomainParticipant where the topic will be created. +* \param[in] topic_desc TopicDescription returned by find_and_check_topic_and_type. +* \param[in] topic_name_mangled Mangled Name of the topic. +* \param[in] options Options of the content filtered topic. +* \param[out] content_filtered_topic Will hold the pointer to the content filtered topic along + with the necessary information for its deletion. +* +* \return true when the content filtered topic was created +* \return false when the content filtered topic could not be created +*/ +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +create_content_filtered_topic( + eprosima::fastdds::dds::DomainParticipant * participant, + eprosima::fastdds::dds::TopicDescription * topic_desc, + const std::string & topic_name_mangled, + const rmw_subscription_content_filter_options_t * options, + eprosima::fastdds::dds::ContentFilteredTopic ** content_filtered_topic); + + +/** +* Create data reader. +* +* \param[in] datareader_qos QoS of data reader. +* \param[in] subscription_options Options of the subscription. +* \param[in] subscriber A subsciber to create the data reader. +* \param[in] des_topic TopicDescription returned by find_and_check_topic_and_type. +* \param[in] listener The listener of the data reader. +* \param[out] data_reader Will hold the pointer to the data reader. +* +* \return true when the data reader was created +* \return false when the data reader could not be created +*/ +RMW_FASTRTPS_SHARED_CPP_PUBLIC +bool +create_datareader( + const eprosima::fastdds::dds::DataReaderQos & datareader_qos, + const rmw_subscription_options_t * subscription_options, + eprosima::fastdds::dds::Subscriber * subscriber, + eprosima::fastdds::dds::TopicDescription * des_topic, + SubListener * listener, + eprosima::fastdds::dds::DataReader ** data_reader); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // RMW_FASTRTPS_SHARED_CPP__UTILS_HPP_ diff --git a/include/rmw_fastrtps_shared_cpp/visibility_control.h b/include/rmw_fastrtps_shared_cpp/visibility_control.h new file mode 100644 index 0000000..de545f6 --- /dev/null +++ b/include/rmw_fastrtps_shared_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_SHARED_CPP__VISIBILITY_CONTROL_H_ +#define RMW_FASTRTPS_SHARED_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_SHARED_CPP_EXPORT __attribute__ ((dllexport)) + #define RMW_FASTRTPS_SHARED_CPP_IMPORT __attribute__ ((dllimport)) + #else + #define RMW_FASTRTPS_SHARED_CPP_EXPORT __declspec(dllexport) + #define RMW_FASTRTPS_SHARED_CPP_IMPORT __declspec(dllimport) + #endif + #ifdef RMW_FASTRTPS_SHARED_CPP_BUILDING_LIBRARY + #define RMW_FASTRTPS_SHARED_CPP_PUBLIC RMW_FASTRTPS_SHARED_CPP_EXPORT + #else + #define RMW_FASTRTPS_SHARED_CPP_PUBLIC RMW_FASTRTPS_SHARED_CPP_IMPORT + #endif + #define RMW_FASTRTPS_SHARED_CPP_PUBLIC_TYPE RMW_FASTRTPS_SHARED_CPP_PUBLIC + #define RMW_FASTRTPS_SHARED_CPP_LOCAL +#else + #define RMW_FASTRTPS_SHARED_CPP_EXPORT __attribute__ ((visibility("default"))) + #define RMW_FASTRTPS_SHARED_CPP_IMPORT + #if __GNUC__ >= 4 + #define RMW_FASTRTPS_SHARED_CPP_PUBLIC __attribute__ ((visibility("default"))) + #define RMW_FASTRTPS_SHARED_CPP_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RMW_FASTRTPS_SHARED_CPP_PUBLIC + #define RMW_FASTRTPS_SHARED_CPP_LOCAL + #endif + #define RMW_FASTRTPS_SHARED_CPP_PUBLIC_TYPE +#endif + +#endif // RMW_FASTRTPS_SHARED_CPP__VISIBILITY_CONTROL_H_ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..49262a6 --- /dev/null +++ b/package.xml @@ -0,0 +1,47 @@ + + + + rmw_fastrtps_shared_cpp + 6.2.1 + Code shared on static and dynamic type support of rmw_fastrtps_cpp. + 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 + rosidl_typesupport_introspection_c + rosidl_typesupport_introspection_cpp + tracetools + + fastcdr + fastrtps + fastrtps_cmake_module + rcpputils + rcutils + rmw + rmw_dds_common + rosidl_typesupport_introspection_c + rosidl_typesupport_introspection_cpp + tracetools + + ament_lint_auto + ament_lint_common + osrf_testing_tools_cpp + + + ament_cmake + + diff --git a/rmw_fastrtps_shared_cpp-extras.cmake b/rmw_fastrtps_shared_cpp-extras.cmake new file mode 100644 index 0000000..860e972 --- /dev/null +++ b/rmw_fastrtps_shared_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_shared_cpp/rmw_fastrtps_shared_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_shared_cpp_INCLUDE_DIRS ${FastRTPS_INCLUDE_DIR}) +# specific order: dependents before dependencies +list(APPEND rmw_fastrtps_shared_cpp_LIBRARIES fastrtps fastcdr) diff --git a/src/TypeSupport_impl.cpp b/src/TypeSupport_impl.cpp new file mode 100644 index 0000000..bad85cb --- /dev/null +++ b/src/TypeSupport_impl.cpp @@ -0,0 +1,591 @@ +// 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 "fastdds/rtps/common/SerializedPayload.h" + +#include "fastcdr/FastBuffer.h" +#include "fastcdr/Cdr.h" + +#include "fastrtps/rtps/common/SerializedPayload.h" +#include "fastrtps/utils/md5.h" +#include "fastrtps/types/TypesBase.h" +#include "fastrtps/types/TypeObjectFactory.h" +#include "fastrtps/types/TypeNamesGenerator.h" +#include "fastrtps/types/AnnotationParameterValue.h" + +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +#include "rmw/error_handling.h" + +#include "rosidl_typesupport_introspection_c/identifier.h" +#include "rosidl_typesupport_introspection_cpp/identifier.hpp" +#include "rosidl_typesupport_introspection_c/message_introspection.h" +#include "rosidl_typesupport_introspection_cpp/message_introspection.hpp" +#include "rosidl_typesupport_introspection_cpp/field_types.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +TypeSupport::TypeSupport() +{ + m_isGetKeyDefined = false; + max_size_bound_ = false; + is_plain_ = false; + auto_fill_type_object(false); + auto_fill_type_information(false); +} + +void TypeSupport::deleteData(void * data) +{ + assert(data); + delete static_cast(data); +} + +void * TypeSupport::createData() +{ + return new eprosima::fastcdr::FastBuffer(); +} + +bool TypeSupport::serialize( + void * data, eprosima::fastrtps::rtps::SerializedPayload_t * payload) +{ + assert(data); + assert(payload); + + auto ser_data = static_cast(data); + if (ser_data->is_cdr_buffer) { + auto ser = static_cast(ser_data->data); + if (payload->max_size >= ser->getSerializedDataLength()) { + payload->length = static_cast(ser->getSerializedDataLength()); + payload->encapsulation = ser->endianness() == + eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + memcpy(payload->data, ser->getBufferPointer(), ser->getSerializedDataLength()); + return true; + } + } else { + eprosima::fastcdr::FastBuffer fastbuffer( + reinterpret_cast(payload->data), + payload->max_size); // Object that manages the raw buffer. + eprosima::fastcdr::Cdr ser(fastbuffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); // Object that serializes the data. + if (this->serializeROSmessage(ser_data->data, ser, ser_data->impl)) { + payload->encapsulation = ser.endianness() == + eprosima::fastcdr::Cdr::BIG_ENDIANNESS ? CDR_BE : CDR_LE; + payload->length = (uint32_t)ser.getSerializedDataLength(); + return true; + } + } + + return false; +} + +bool TypeSupport::deserialize( + eprosima::fastrtps::rtps::SerializedPayload_t * payload, + void * data) +{ + assert(data); + assert(payload); + + auto ser_data = static_cast(data); + if (ser_data->is_cdr_buffer) { + auto buffer = static_cast(ser_data->data); + if (!buffer->reserve(payload->length)) { + return false; + } + memcpy(buffer->getBuffer(), payload->data, payload->length); + return true; + } + + eprosima::fastcdr::FastBuffer fastbuffer( + reinterpret_cast(payload->data), + payload->length); + eprosima::fastcdr::Cdr deser( + fastbuffer, + eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + return deserializeROSmessage(deser, ser_data->data, ser_data->impl); +} + +std::function TypeSupport::getSerializedSizeProvider(void * data) +{ + assert(data); + + auto ser_data = static_cast(data); + auto ser_size = [this, ser_data]() -> uint32_t + { + if (ser_data->is_cdr_buffer) { + auto ser = static_cast(ser_data->data); + return static_cast(ser->getSerializedDataLength()); + } + return static_cast( + this->getEstimatedSerializedSize( + ser_data->data, + ser_data->impl)); + }; + return ser_size; +} + +// TODO(iuhilnehc-ynos): add the following content into new files named TypeObject? +using CompleteStructType = eprosima::fastrtps::types::CompleteStructType; +using CompleteStructMember = eprosima::fastrtps::types::CompleteStructMember; +using MinimalStructType = eprosima::fastrtps::types::MinimalStructType; +using MinimalStructMember = eprosima::fastrtps::types::MinimalStructMember; +using SerializedPayload_t = eprosima::fastrtps::rtps::SerializedPayload_t; +using TypeNamesGenerator = eprosima::fastrtps::types::TypeNamesGenerator; +using TypeIdentifier = eprosima::fastrtps::types::TypeIdentifier; +using TypeObject = eprosima::fastrtps::types::TypeObject; +using TypeObjectFactory = eprosima::fastrtps::types::TypeObjectFactory; + +const rosidl_message_type_support_t * +get_type_support_introspection( + const rosidl_message_type_support_t * type_supports) +{ + const rosidl_message_type_support_t * type_support = + get_message_typesupport_handle( + type_supports, rosidl_typesupport_introspection_c__identifier); + if (nullptr == 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 (nullptr == 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; + } + } + + return type_support; +} + +template +inline std::string +_create_type_name( + const MembersType * members) +{ + if (!members) { + return std::string(); + } + + std::ostringstream ss; + std::string message_namespace(members->message_namespace_); + std::string message_name(members->message_name_); + if (!message_namespace.empty()) { + ss << message_namespace << "::"; + } + ss << "dds_::" << message_name << "_"; + return ss.str(); +} + +typedef std::pair MemberIdentifierName; + +template +MemberIdentifierName GetTypeIdentifier(const MembersType * member, uint32_t index, bool complete); + +template +const TypeObject * GetCompleteObject( + const std::string & type_name, + const MembersType * members) +{ + const TypeObject * c_type_object = + TypeObjectFactory::get_instance()->get_type_object(type_name, true); + if (c_type_object != nullptr && c_type_object->_d() == eprosima::fastrtps::types::EK_COMPLETE) { + return c_type_object; + } + + TypeObject * type_object = new TypeObject(); + + type_object->_d(eprosima::fastrtps::types::EK_COMPLETE); + type_object->complete()._d(eprosima::fastrtps::types::TK_STRUCTURE); + type_object->complete().struct_type().struct_flags().IS_FINAL(false); + type_object->complete().struct_type().struct_flags().IS_APPENDABLE(false); + type_object->complete().struct_type().struct_flags().IS_MUTABLE(false); + // Not sure whether current type is nested or not, make all Type Nested + type_object->complete().struct_type().struct_flags().IS_NESTED(true); + type_object->complete().struct_type().struct_flags().IS_AUTOID_HASH(false); // Unsupported + + for (uint32_t i = 0; i < members->member_count_; ++i) { + CompleteStructMember cst_field; + cst_field.common().member_id(i); + cst_field.common().member_flags().TRY_CONSTRUCT1(false); // Unsupported + cst_field.common().member_flags().TRY_CONSTRUCT2(false); // Unsupported + cst_field.common().member_flags().IS_EXTERNAL(false); // Unsupported + cst_field.common().member_flags().IS_OPTIONAL(false); + cst_field.common().member_flags().IS_MUST_UNDERSTAND(false); + cst_field.common().member_flags().IS_KEY(false); + cst_field.common().member_flags().IS_DEFAULT(false); // Doesn't apply + + MemberIdentifierName pair = GetTypeIdentifier(members, i, true); + if (!pair.first) { + continue; + } + cst_field.common().member_type_id(*pair.first); + cst_field.detail().name(pair.second); + type_object->complete().struct_type().member_seq().emplace_back(cst_field); + } + + // Header + type_object->complete().struct_type().header().detail().type_name(type_name); + + TypeIdentifier identifier; + identifier._d(eprosima::fastrtps::types::EK_COMPLETE); + + SerializedPayload_t payload(static_cast( + CompleteStructType::getCdrSerializedSize(type_object->complete().struct_type()) + 4)); + + eprosima::fastcdr::FastBuffer fastbuffer( + reinterpret_cast(payload.data), payload.max_size); + + // Fixed endian (Page 221, EquivalenceHash definition of Extensible and Dynamic Topic Types for + // DDS document) + eprosima::fastcdr::Cdr ser( + fastbuffer, eprosima::fastcdr::Cdr::LITTLE_ENDIANNESS, + eprosima::fastcdr::Cdr::DDS_CDR); // Object that serializes the data. + payload.encapsulation = CDR_LE; + + type_object->serialize(ser); + payload.length = + static_cast(ser.getSerializedDataLength()); // Get the serialized length + MD5 objectHash; + objectHash.update(reinterpret_cast(payload.data), payload.length); + objectHash.finalize(); + for (int i = 0; i < 14; ++i) { + identifier.equivalence_hash()[i] = objectHash.digest[i]; + } + + TypeObjectFactory::get_instance()->add_type_object(type_name, &identifier, type_object); + delete type_object; + + return TypeObjectFactory::get_instance()->get_type_object(type_name, true); +} + +template +const TypeObject * GetMinimalObject( + const std::string & type_name, + const MembersType * members) +{ + const TypeObject * c_type_object = + TypeObjectFactory::get_instance()->get_type_object(type_name, false); + if (c_type_object != nullptr) { + return c_type_object; + } + + TypeObject * type_object = new TypeObject(); + type_object->_d(eprosima::fastrtps::types::EK_MINIMAL); + type_object->minimal()._d(eprosima::fastrtps::types::TK_STRUCTURE); + type_object->minimal().struct_type().struct_flags().IS_FINAL(false); + type_object->minimal().struct_type().struct_flags().IS_APPENDABLE(false); + type_object->minimal().struct_type().struct_flags().IS_MUTABLE(false); + type_object->minimal().struct_type().struct_flags().IS_NESTED(true); + type_object->minimal().struct_type().struct_flags().IS_AUTOID_HASH(false); // Unsupported + + for (uint32_t i = 0; i < members->member_count_; ++i) { + MinimalStructMember mst_field; + mst_field.common().member_id(i); + mst_field.common().member_flags().TRY_CONSTRUCT1(false); // Unsupported + mst_field.common().member_flags().TRY_CONSTRUCT2(false); // Unsupported + mst_field.common().member_flags().IS_EXTERNAL(false); // Unsupported + mst_field.common().member_flags().IS_OPTIONAL(false); + mst_field.common().member_flags().IS_MUST_UNDERSTAND(false); + mst_field.common().member_flags().IS_KEY(false); + mst_field.common().member_flags().IS_DEFAULT(false); // Doesn't apply + + MemberIdentifierName pair = GetTypeIdentifier(members, i, false); + if (!pair.first) { + continue; + } + mst_field.common().member_type_id(*pair.first); + MD5 field_hash(pair.second); + for (int i = 0; i < 4; ++i) { + mst_field.detail().name_hash()[i] = field_hash.digest[i]; + } + type_object->minimal().struct_type().member_seq().emplace_back(mst_field); + } + + TypeIdentifier identifier; + identifier._d(eprosima::fastrtps::types::EK_MINIMAL); + + SerializedPayload_t payload( + static_cast( + MinimalStructType::getCdrSerializedSize(type_object->minimal().struct_type()) + 4)); + + eprosima::fastcdr::FastBuffer fastbuffer( + reinterpret_cast(payload.data), payload.max_size); + + // Fixed endian (Page 221, EquivalenceHash definition of Extensible and Dynamic Topic Types for + // DDS document) + eprosima::fastcdr::Cdr ser( + fastbuffer, eprosima::fastcdr::Cdr::LITTLE_ENDIANNESS, + eprosima::fastcdr::Cdr::DDS_CDR); // Object that serializes the data. + payload.encapsulation = CDR_LE; + + type_object->serialize(ser); + payload.length = + static_cast(ser.getSerializedDataLength()); // Get the serialized length + MD5 objectHash; + objectHash.update(reinterpret_cast(payload.data), payload.length); + objectHash.finalize(); + for (int i = 0; i < 14; ++i) { + identifier.equivalence_hash()[i] = objectHash.digest[i]; + } + + TypeObjectFactory::get_instance()->add_type_object(type_name, &identifier, type_object); + delete type_object; + return TypeObjectFactory::get_instance()->get_type_object(type_name, false); +} + +template +MemberIdentifierName GetTypeIdentifier(const MembersType * members, uint32_t index, bool complete) +{ + const auto member = members->members_ + index; + const TypeIdentifier * type_identifier = nullptr; + std::string name = member->name_; + + std::string type_name; + bool complete_type = false; + switch (member->type_id_) { + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_FLOAT: + { + type_name = "float"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_DOUBLE: + { + type_name = "double"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_LONG_DOUBLE: + { + type_name = "longdouble"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_CHAR: + { + type_name = "char"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WCHAR: + { + type_name = "wchar"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_BOOLEAN: + { + type_name = "bool"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_OCTET: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT8: + { + type_name = "uint8_t"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT8: + { + type_name = "int8_t"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT16: + { + type_name = "uint16_t"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT16: + { + type_name = "int16_t"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT32: + { + type_name = "uint32_t"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT32: + { + type_name = "int32_t"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_UINT64: + { + type_name = "uint64_t"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_INT64: + { + type_name = "int64_t"; + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING: + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_WSTRING: + { + uint32_t bound = member->string_upper_bound_ ? + static_cast(member->string_upper_bound_) : 255; + bool wide = + (member->type_id_ == ::rosidl_typesupport_introspection_cpp::ROS_TYPE_STRING) ? + false : true; + TypeObjectFactory::get_instance()->get_string_identifier(bound, wide); + type_name = TypeNamesGenerator::get_string_type_name( + bound, wide); + break; + } + case ::rosidl_typesupport_introspection_cpp::ROS_TYPE_MESSAGE: + { + const rosidl_message_type_support_t * type_support_intro = + get_type_support_introspection(member->members_); + const MembersType * sub_members = + static_cast(type_support_intro->data); + std::string sub_type_name = _create_type_name(sub_members); + if (complete) { + GetCompleteObject(sub_type_name, sub_members); + } else { + GetMinimalObject(sub_type_name, sub_members); + } + type_name = sub_type_name; + complete_type = complete; + } + break; + default: + break; + } + + if (!type_name.empty()) { + if (!member->is_array_) { + type_identifier = TypeObjectFactory::get_instance()->get_type_identifier( + type_name, complete_type); + } else if (member->array_size_ && !member->is_upper_bound_) { + type_identifier = TypeObjectFactory::get_instance()->get_array_identifier( + type_name, {static_cast(member->array_size_)}, complete_type); + } else { + type_identifier = TypeObjectFactory::get_instance()->get_sequence_identifier( + type_name, 0, complete_type); + } + } + + return {type_identifier, name}; +} + +template +const TypeObject * GetTypeObject( + const std::string & type_name, bool complete, + const MembersType * members) +{ + const TypeObject * c_type_object = + TypeObjectFactory::get_instance()->get_type_object(type_name, complete); + if (c_type_object != nullptr) { + return c_type_object; + } else if (complete) { + return GetCompleteObject(type_name, members); + } + // else + return GetMinimalObject(type_name, members); +} + +template +const TypeIdentifier * GetTypeIdentifier( + const std::string & type_name, bool complete, + const MembersType * members) +{ + const TypeIdentifier * c_identifier = + TypeObjectFactory::get_instance()->get_type_identifier(type_name, complete); + if (c_identifier != nullptr && + (!complete || c_identifier->_d() == eprosima::fastrtps::types::EK_COMPLETE)) + { + return c_identifier; + } + + GetTypeObject(type_name, complete, members); // Generated inside + return TypeObjectFactory::get_instance()->get_type_identifier(type_name, complete); +} + +template +inline bool +add_type_object( + const void * untype_members, + const std::string & type_name) +{ + const MembersType * members = static_cast(untype_members); + if (!members) { + return false; + } + + TypeObjectFactory * factory = TypeObjectFactory::get_instance(); + if (!factory) { + return false; + } + + const TypeIdentifier * identifier = nullptr; + const TypeObject * type_object = nullptr; + identifier = GetTypeIdentifier(type_name, true, members); + if (!identifier) { + return false; + } + type_object = GetTypeObject(type_name, true, members); + if (!type_object) { + return false; + } + + factory->add_type_object(type_name, identifier, type_object); + + identifier = GetTypeIdentifier(type_name, false, members); + if (!identifier) { + return false; + } + type_object = GetTypeObject(type_name, false, members); + if (!type_object) { + return false; + } + factory->add_type_object(type_name, identifier, type_object); + + return true; +} + +bool register_type_object( + const rosidl_message_type_support_t * type_supports, + const std::string & type_name) +{ + const rosidl_message_type_support_t * type_support_intro = + get_type_support_introspection(type_supports); + if (!type_support_intro) { + return false; + } + + bool ret = false; + if (type_support_intro->typesupport_identifier == + rosidl_typesupport_introspection_c__identifier) + { + ret = add_type_object( + type_support_intro->data, type_name); + } else { + ret = add_type_object( + type_support_intro->data, type_name); + } + + return ret; +} + +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/create_rmw_gid.cpp b/src/create_rmw_gid.cpp new file mode 100644 index 0000000..49b9049 --- /dev/null +++ b/src/create_rmw_gid.cpp @@ -0,0 +1,36 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "fastdds/rtps/common/Guid.h" + +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/create_rmw_gid.hpp" +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" + +rmw_gid_t +rmw_fastrtps_shared_cpp::create_rmw_gid( + const char * identifier, const eprosima::fastrtps::rtps::GUID_t & guid) +{ + rmw_gid_t rmw_gid = {}; + rmw_gid.implementation_identifier = identifier; + static_assert( + sizeof(eprosima::fastrtps::rtps::GUID_t) <= RMW_GID_STORAGE_SIZE, + "RMW_GID_STORAGE_SIZE insufficient to store the fastrtps GUID_t." + ); + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + guid, + rmw_gid.data); + return rmw_gid; +} diff --git a/src/custom_publisher_info.cpp b/src/custom_publisher_info.cpp new file mode 100644 index 0000000..9fc6e40 --- /dev/null +++ b/src/custom_publisher_info.cpp @@ -0,0 +1,176 @@ +// 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_fastrtps_shared_cpp/custom_publisher_info.hpp" + +#include "fastdds/dds/core/status/BaseStatus.hpp" +#include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" + +#include "event_helpers.hpp" +#include "types/event_types.hpp" + +EventListenerInterface * +CustomPublisherInfo::getListener() const +{ + return listener_; +} + +void +PubListener::on_offered_deadline_missed( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::OfferedDeadlineMissedStatus & status) +{ + std::lock_guard lock(internalMutex_); + + // the change to liveliness_lost_count_ needs to be mutually exclusive with + // rmw_wait() which checks hasEvent() and decides if wait() needs to be called + ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + + // Assign absolute values + offered_deadline_missed_status_.total_count = status.total_count; + // Accumulate deltas + offered_deadline_missed_status_.total_count_change += status.total_count_change; + + deadline_changes_.store(true, std::memory_order_relaxed); + + std::unique_lock lock_mutex(on_new_event_m_); + + if (on_new_event_cb_) { + on_new_event_cb_(user_data_, 1); + } else { + unread_events_count_++; + } +} + +void PubListener::on_liveliness_lost( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::LivelinessLostStatus & status) +{ + std::lock_guard lock(internalMutex_); + + // the change to liveliness_lost_count_ needs to be mutually exclusive with + // rmw_wait() which checks hasEvent() and decides if wait() needs to be called + ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + + // Assign absolute values + liveliness_lost_status_.total_count = status.total_count; + // Accumulate deltas + liveliness_lost_status_.total_count_change += status.total_count_change; + + liveliness_changes_.store(true, std::memory_order_relaxed); + + std::unique_lock lock_mutex(on_new_event_m_); + + if (on_new_event_cb_) { + on_new_event_cb_(user_data_, 1); + } else { + unread_events_count_++; + } +} + +void PubListener::on_offered_incompatible_qos( + eprosima::fastdds::dds::DataWriter * /* writer */, + const eprosima::fastdds::dds::OfferedIncompatibleQosStatus & status) +{ + std::lock_guard lock(internalMutex_); + + // the change to incompatible_qos_status_ needs to be mutually exclusive with + // rmw_wait() which checks hasEvent() and decides if wait() needs to be called + ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + + // Assign absolute values + incompatible_qos_status_.last_policy_id = status.last_policy_id; + incompatible_qos_status_.total_count = status.total_count; + // Accumulate deltas + incompatible_qos_status_.total_count_change += status.total_count_change; + + incompatible_qos_changes_.store(true, std::memory_order_relaxed); +} + +bool PubListener::hasEvent(rmw_event_type_t event_type) const +{ + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + switch (event_type) { + case RMW_EVENT_LIVELINESS_LOST: + return liveliness_changes_.load(std::memory_order_relaxed); + case RMW_EVENT_OFFERED_DEADLINE_MISSED: + return deadline_changes_.load(std::memory_order_relaxed); + case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: + return incompatible_qos_changes_.load(std::memory_order_relaxed); + default: + break; + } + return false; +} + +void PubListener::set_on_new_event_callback( + const void * user_data, + rmw_event_callback_t callback) +{ + std::unique_lock lock_mutex(on_new_event_m_); + + if (callback) { + // Push events arrived before setting the executor's callback + if (unread_events_count_) { + callback(user_data, unread_events_count_); + unread_events_count_ = 0; + } + user_data_ = user_data; + on_new_event_cb_ = callback; + } else { + user_data_ = nullptr; + on_new_event_cb_ = nullptr; + } +} + +bool PubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) +{ + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + std::lock_guard lock(internalMutex_); + switch (event_type) { + case RMW_EVENT_LIVELINESS_LOST: + { + auto rmw_data = static_cast(event_info); + rmw_data->total_count = liveliness_lost_status_.total_count; + rmw_data->total_count_change = liveliness_lost_status_.total_count_change; + liveliness_lost_status_.total_count_change = 0; + liveliness_changes_.store(false, std::memory_order_relaxed); + } + break; + case RMW_EVENT_OFFERED_DEADLINE_MISSED: + { + auto rmw_data = static_cast(event_info); + rmw_data->total_count = offered_deadline_missed_status_.total_count; + rmw_data->total_count_change = offered_deadline_missed_status_.total_count_change; + offered_deadline_missed_status_.total_count_change = 0; + deadline_changes_.store(false, std::memory_order_relaxed); + } + break; + case RMW_EVENT_OFFERED_QOS_INCOMPATIBLE: + { + auto rmw_data = static_cast(event_info); + rmw_data->total_count = incompatible_qos_status_.total_count; + rmw_data->total_count_change = incompatible_qos_status_.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + incompatible_qos_status_.last_policy_id); + incompatible_qos_status_.total_count_change = 0; + incompatible_qos_changes_.store(false, std::memory_order_relaxed); + } + break; + default: + return false; + } + return true; +} diff --git a/src/custom_subscriber_info.cpp b/src/custom_subscriber_info.cpp new file mode 100644 index 0000000..6c24c35 --- /dev/null +++ b/src/custom_subscriber_info.cpp @@ -0,0 +1,211 @@ +// 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_fastrtps_shared_cpp/custom_subscriber_info.hpp" + +#include "fastdds/dds/core/status/DeadlineMissedStatus.hpp" +#include "fastdds/dds/core/status/LivelinessChangedStatus.hpp" + +#include "event_helpers.hpp" +#include "types/event_types.hpp" + +EventListenerInterface * +CustomSubscriberInfo::getListener() const +{ + return listener_; +} + +void +SubListener::on_requested_deadline_missed( + eprosima::fastdds::dds::DataReader * /* reader */, + const eprosima::fastdds::dds::RequestedDeadlineMissedStatus & status) +{ + std::lock_guard lock(internalMutex_); + + // the change to liveliness_lost_count_ needs to be mutually exclusive with + // rmw_wait() which checks hasEvent() and decides if wait() needs to be called + ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + + // Assign absolute values + requested_deadline_missed_status_.total_count = status.total_count; + // Accumulate deltas + requested_deadline_missed_status_.total_count_change += status.total_count_change; + + deadline_changes_.store(true, std::memory_order_relaxed); + + std::unique_lock lock_mutex(on_new_event_m_); + + if (on_new_event_cb_) { + on_new_event_cb_(user_data_, 1); + } else { + unread_events_count_++; + } +} + +void SubListener::on_liveliness_changed( + eprosima::fastdds::dds::DataReader * /* reader */, + const eprosima::fastdds::dds::LivelinessChangedStatus & status) +{ + std::lock_guard lock(internalMutex_); + + // the change to liveliness_lost_count_ needs to be mutually exclusive with + // rmw_wait() which checks hasEvent() and decides if wait() needs to be called + ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + + // Assign absolute values + liveliness_changed_status_.alive_count = status.alive_count; + liveliness_changed_status_.not_alive_count = status.not_alive_count; + // Accumulate deltas + liveliness_changed_status_.alive_count_change += status.alive_count_change; + liveliness_changed_status_.not_alive_count_change += status.not_alive_count_change; + + liveliness_changes_.store(true, std::memory_order_relaxed); + + std::unique_lock lock_mutex(on_new_event_m_); + + if (on_new_event_cb_) { + on_new_event_cb_(user_data_, 1); + } else { + unread_events_count_++; + } +} + +void SubListener::on_sample_lost( + eprosima::fastdds::dds::DataReader * /* reader */, + const eprosima::fastdds::dds::SampleLostStatus & status) +{ + std::lock_guard lock(internalMutex_); + + // the change to sample_lost_status_ needs to be mutually exclusive with + // rmw_wait() which checks hasEvent() and decides if wait() needs to be called + ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + + // Assign absolute values + sample_lost_status_.total_count = status.total_count; + // Accumulate deltas + sample_lost_status_.total_count_change += status.total_count_change; + + sample_lost_changes_.store(true, std::memory_order_relaxed); +} + +void SubListener::on_requested_incompatible_qos( + eprosima::fastdds::dds::DataReader * /* reader */, + const eprosima::fastdds::dds::RequestedIncompatibleQosStatus & status) +{ + std::lock_guard lock(internalMutex_); + + // the change to incompatible_qos_status_ needs to be mutually exclusive with + // rmw_wait() which checks hasEvent() and decides if wait() needs to be called + ConditionalScopedLock clock(conditionMutex_, conditionVariable_); + + // Assign absolute values + incompatible_qos_status_.last_policy_id = status.last_policy_id; + incompatible_qos_status_.total_count = status.total_count; + // Accumulate deltas + incompatible_qos_status_.total_count_change += status.total_count_change; + + incompatible_qos_changes_.store(true, std::memory_order_relaxed); +} + +bool SubListener::hasEvent(rmw_event_type_t event_type) const +{ + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + switch (event_type) { + case RMW_EVENT_LIVELINESS_CHANGED: + return liveliness_changes_.load(std::memory_order_relaxed); + case RMW_EVENT_REQUESTED_DEADLINE_MISSED: + return deadline_changes_.load(std::memory_order_relaxed); + case RMW_EVENT_MESSAGE_LOST: + return sample_lost_changes_.load(std::memory_order_relaxed); + case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: + return incompatible_qos_changes_.load(std::memory_order_relaxed); + default: + break; + } + return false; +} + +void SubListener::set_on_new_event_callback( + const void * user_data, + rmw_event_callback_t callback) +{ + std::unique_lock lock_mutex(on_new_event_m_); + + if (callback) { + // Push events arrived before setting the executor's callback + if (unread_events_count_) { + callback(user_data, unread_events_count_); + unread_events_count_ = 0; + } + user_data_ = user_data; + on_new_event_cb_ = callback; + } else { + user_data_ = nullptr; + on_new_event_cb_ = nullptr; + return; + } +} + +bool SubListener::takeNextEvent(rmw_event_type_t event_type, void * event_info) +{ + assert(rmw_fastrtps_shared_cpp::internal::is_event_supported(event_type)); + std::lock_guard lock(internalMutex_); + switch (event_type) { + case RMW_EVENT_LIVELINESS_CHANGED: + { + auto rmw_data = static_cast(event_info); + rmw_data->alive_count = liveliness_changed_status_.alive_count; + rmw_data->not_alive_count = liveliness_changed_status_.not_alive_count; + rmw_data->alive_count_change = liveliness_changed_status_.alive_count_change; + rmw_data->not_alive_count_change = liveliness_changed_status_.not_alive_count_change; + liveliness_changed_status_.alive_count_change = 0; + liveliness_changed_status_.not_alive_count_change = 0; + liveliness_changes_.store(false, std::memory_order_relaxed); + } + break; + case RMW_EVENT_REQUESTED_DEADLINE_MISSED: + { + auto rmw_data = static_cast(event_info); + rmw_data->total_count = requested_deadline_missed_status_.total_count; + rmw_data->total_count_change = requested_deadline_missed_status_.total_count_change; + requested_deadline_missed_status_.total_count_change = 0; + deadline_changes_.store(false, std::memory_order_relaxed); + } + break; + case RMW_EVENT_MESSAGE_LOST: + { + auto rmw_data = static_cast(event_info); + rmw_data->total_count = sample_lost_status_.total_count; + rmw_data->total_count_change = sample_lost_status_.total_count_change; + sample_lost_status_.total_count_change = 0; + sample_lost_changes_.store(false, std::memory_order_relaxed); + } + break; + case RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE: + { + auto rmw_data = static_cast(event_info); + rmw_data->total_count = incompatible_qos_status_.total_count; + rmw_data->total_count_change = incompatible_qos_status_.total_count_change; + rmw_data->last_policy_kind = + rmw_fastrtps_shared_cpp::internal::dds_qos_policy_to_rmw_qos_policy( + incompatible_qos_status_.last_policy_id); + incompatible_qos_status_.total_count_change = 0; + incompatible_qos_changes_.store(false, std::memory_order_relaxed); + } + break; + default: + return false; + } + return true; +} diff --git a/src/demangle.cpp b/src/demangle.cpp new file mode 100644 index 0000000..15e4e3f --- /dev/null +++ b/src/demangle.cpp @@ -0,0 +1,167 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "rcpputils/find_and_replace.hpp" +#include "rcutils/logging_macros.h" +#include "rcutils/types.h" + +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" + +#include "demangle.hpp" + +/// Return the demangle ROS topic or the original if not a ROS topic. +std::string +_demangle_if_ros_topic(const std::string & topic_name) +{ + return _strip_ros_prefix_if_exists(topic_name); +} + +/// Return the demangled ROS type or the original if not a ROS type. +std::string +_demangle_if_ros_type(const std::string & dds_type_string) +{ + if (dds_type_string[dds_type_string.size() - 1] != '_') { + // not a ROS type + return dds_type_string; + } + + std::string substring = "dds_::"; + size_t substring_position = dds_type_string.find(substring); + if (substring_position == std::string::npos) { + // not a ROS type + return dds_type_string; + } + + std::string type_namespace = dds_type_string.substr(0, substring_position); + type_namespace = rcpputils::find_and_replace(type_namespace, "::", "/"); + size_t start = substring_position + substring.size(); + std::string type_name = dds_type_string.substr(start, dds_type_string.length() - 1 - start); + return type_namespace + type_name; +} + +/// Return the topic name for a given topic if it is part of one, else "". +std::string +_demangle_ros_topic_from_topic(const std::string & topic_name) +{ + return _resolve_prefix(topic_name, ros_topic_prefix); +} + +/// Return the service name for a given topic if it is part of one, else "". +std::string +_demangle_service_from_topic( + const std::string & prefix, const std::string & topic_name, std::string suffix) +{ + std::string service_name = _resolve_prefix(topic_name, prefix); + if ("" == service_name) { + return ""; + } + + size_t suffix_position = service_name.rfind(suffix); + if (suffix_position != std::string::npos) { + if (service_name.length() - suffix_position - suffix.length() != 0) { + RCUTILS_LOG_WARN_NAMED( + "rmw_fastrtps_shared_cpp", + "service topic has service prefix and a suffix, but not at the end" + ", report this: '%s'", topic_name.c_str()); + return ""; + } + } else { + RCUTILS_LOG_WARN_NAMED( + "rmw_fastrtps_shared_cpp", + "service topic has prefix but no suffix" + ", report this: '%s'", topic_name.c_str()); + return ""; + } + return service_name.substr(0, suffix_position); +} + +std::string +_demangle_service_from_topic(const std::string & topic_name) +{ + const std::string demangled_topic = _demangle_service_reply_from_topic(topic_name); + if ("" != demangled_topic) { + return demangled_topic; + } + return _demangle_service_request_from_topic(topic_name); +} + + +std::string +_demangle_service_request_from_topic(const std::string & topic_name) +{ + return _demangle_service_from_topic(ros_service_requester_prefix, topic_name, "Request"); +} + +std::string +_demangle_service_reply_from_topic(const std::string & topic_name) +{ + return _demangle_service_from_topic(ros_service_response_prefix, topic_name, "Reply"); +} + +/// Return the demangled service type if it is a ROS srv type, else "". +std::string +_demangle_service_type_only(const std::string & dds_type_name) +{ + std::string ns_substring = "dds_::"; + size_t ns_substring_position = dds_type_name.find(ns_substring); + if (std::string::npos == ns_substring_position) { + // not a ROS service type + return ""; + } + auto suffixes = { + std::string("_Response_"), + std::string("_Request_"), + }; + std::string found_suffix = ""; + size_t suffix_position = 0; + for (auto suffix : suffixes) { + suffix_position = dds_type_name.rfind(suffix); + if (suffix_position != std::string::npos) { + if (dds_type_name.length() - suffix_position - suffix.length() != 0) { + RCUTILS_LOG_WARN_NAMED( + "rmw_fastrtps_shared_cpp", + "service type contains 'dds_::' and a suffix, but not at the end" + ", report this: '%s'", dds_type_name.c_str()); + continue; + } + found_suffix = suffix; + break; + } + } + if (std::string::npos == suffix_position) { + RCUTILS_LOG_WARN_NAMED( + "rmw_fastrtps_shared_cpp", + "service type contains 'dds_::' but does not have a suffix" + ", report this: '%s'", dds_type_name.c_str()); + return ""; + } + // everything checks out, reformat it from '[type_namespace::]dds_::' + // to '[type_namespace/]' + std::string type_namespace = dds_type_name.substr(0, ns_substring_position); + type_namespace = rcpputils::find_and_replace(type_namespace, "::", "/"); + size_t start = ns_substring_position + ns_substring.length(); + std::string type_name = dds_type_name.substr(start, suffix_position - start); + return type_namespace + type_name; +} + +std::string +_identity_demangle(const std::string & name) +{ + return name; +} diff --git a/src/demangle.hpp b/src/demangle.hpp new file mode 100644 index 0000000..02a3e00 --- /dev/null +++ b/src/demangle.hpp @@ -0,0 +1,57 @@ +// 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. + +#ifndef DEMANGLE_HPP_ +#define DEMANGLE_HPP_ + +#include + +/// Return the demangle ROS topic or the original if not a ROS topic. +std::string +_demangle_if_ros_topic(const std::string & topic_name); + +/// Return the demangled ROS type or the original if not a ROS type. +std::string +_demangle_if_ros_type(const std::string & dds_type_string); + +/// Return the topic name for a given topic if it is part of one, else "". +std::string +_demangle_ros_topic_from_topic(const std::string & topic_name); + +/// Return the service name for a given topic if it is part of a service, else "". +std::string +_demangle_service_from_topic(const std::string & topic_name); + +/// Return the service name for a given topic if it is part of a service request, else "". +std::string +_demangle_service_request_from_topic(const std::string & topic_name); + +/// Return the service name for a given topic if it is part of a service reply, else "". +std::string +_demangle_service_reply_from_topic(const std::string & topic_name); + +/// Return the demangled service type if it is a ROS srv type, else "". +std::string +_demangle_service_type_only(const std::string & dds_type_name); + +/// Used when ros names are not mangled. +std::string +_identity_demangle(const std::string & name); + + +using DemangleFunction = std::string (*)(const std::string &); +using MangleFunction = DemangleFunction; + +#endif // DEMANGLE_HPP_ diff --git a/src/event_helpers.hpp b/src/event_helpers.hpp new file mode 100644 index 0000000..e667f96 --- /dev/null +++ b/src/event_helpers.hpp @@ -0,0 +1,33 @@ +// 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. + +#ifndef EVENT_HELPERS_HPP_ +#define EVENT_HELPERS_HPP_ + +#include "fastdds/dds/core/policy/QosPolicies.hpp" + +#include "rmw/qos_policy_kind.h" + +namespace rmw_fastrtps_shared_cpp +{ +namespace internal +{ + +rmw_qos_policy_kind_t dds_qos_policy_to_rmw_qos_policy( + eprosima::fastdds::dds::QosPolicyId_t policy_id); + +} // namespace internal +} // namespace rmw_fastrtps_shared_cpp + +#endif // EVENT_HELPERS_HPP_ diff --git a/src/init_rmw_context_impl.cpp b/src/init_rmw_context_impl.cpp new file mode 100644 index 0000000..fc3b0db --- /dev/null +++ b/src/init_rmw_context_impl.cpp @@ -0,0 +1,109 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw_fastrtps_shared_cpp/init_rmw_context_impl.hpp" + +#include + +#include "rmw/error_handling.h" +#include "rmw/init.h" + +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/participant.hpp" +#include "rmw_fastrtps_shared_cpp/publisher.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +#include "rmw_fastrtps_shared_cpp/listener_thread.hpp" + +rmw_ret_t +rmw_fastrtps_shared_cpp::decrement_context_impl_ref_count(rmw_context_t * context) +{ + std::lock_guard guard(context->impl->mutex); + + assert(context); + assert(context->impl); + assert(0u < context->impl->count); + + if (--context->impl->count > 0) { + return RMW_RET_OK; + } + + rmw_ret_t err = RMW_RET_OK; + rmw_ret_t ret = RMW_RET_OK; + rmw_error_string_t error_string; + + ret = rmw_fastrtps_shared_cpp::join_listener_thread(context); + if (RMW_RET_OK != ret) { + return ret; + } + + auto common_context = static_cast(context->impl->common); + auto participant_info = static_cast(context->impl->participant_info); + + if (!common_context->graph_cache.remove_participant(common_context->gid)) { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " + "couldn't remove Participant gid from graph_cache when destroying Participant"); + } + + ret = rmw_fastrtps_shared_cpp::destroy_subscription( + context->implementation_identifier, + participant_info, + common_context->sub); + // Try to clean the other objects if the above failed. + if (RMW_RET_OK != ret) { + error_string = rmw_get_error_string(); + rmw_reset_error(); + } + err = rmw_fastrtps_shared_cpp::destroy_publisher( + context->implementation_identifier, + participant_info, + common_context->pub); + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + // We can just return one error, log about the previous one. + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_subscription' failed\n"); + ret = err; + error_string = rmw_get_error_string(); + rmw_reset_error(); + } + err = rmw_fastrtps_shared_cpp::destroy_participant(participant_info); + if (RMW_RET_OK != ret && RMW_RET_OK != err) { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__LINE__) + ": 'destroy_publisher' failed\n"); + ret = err; + } else if (RMW_RET_OK != ret) { + RMW_SET_ERROR_MSG(error_string.str); + } + + common_context->graph_cache.clear_on_change_callback(); + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->graph_guard_condition)) + { + RMW_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__function__) ":" RCUTILS_STRINGIFY(__line__) ": " + "couldn't destroy graph_guard_condtion"); + } + + delete common_context; + context->impl->common = nullptr; + context->impl->participant_info = nullptr; + return ret; +} diff --git a/src/listener_thread.cpp b/src/listener_thread.cpp new file mode 100644 index 0000000..b659dd4 --- /dev/null +++ b/src/listener_thread.cpp @@ -0,0 +1,180 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "rcutils/macros.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/init.h" +#include "rmw/ret_types.h" +#include "rmw/rmw.h" +#include "rmw/types.h" +#include "rmw/impl/cpp/macros.hpp" + +#include "rmw_dds_common/context.hpp" +#include "rmw_dds_common/gid_utils.hpp" +#include "rmw_dds_common/msg/participant_entities_info.hpp" + +#include "rmw_fastrtps_shared_cpp/listener_thread.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +using rmw_dds_common::operator<<; + +static +void +node_listener(rmw_context_t * context); + +rmw_ret_t +rmw_fastrtps_shared_cpp::run_listener_thread(rmw_context_t * context) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); + + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.store(true); + common_context->listener_thread_gc = rmw_fastrtps_shared_cpp::__rmw_create_guard_condition( + context->implementation_identifier); + if (common_context->listener_thread_gc) { + try { + common_context->listener_thread = std::thread(node_listener, context); + return RMW_RET_OK; + } catch (const std::exception & exc) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to create std::thread: %s", exc.what()); + } catch (...) { + RMW_SET_ERROR_MSG("Failed to create std::thread"); + } + } else { + RMW_SET_ERROR_MSG("Failed to create guard condition"); + } + common_context->thread_is_running.store(false); + if (common_context->listener_thread_gc) { + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->listener_thread_gc)) + { + RCUTILS_SAFE_FWRITE_TO_STDERR( + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" + RCUTILS_STRINGIFY(__LINE__) ": Failed to destroy guard condition"); + } + } + return RMW_RET_ERROR; +} + +rmw_ret_t +rmw_fastrtps_shared_cpp::join_listener_thread(rmw_context_t * context) +{ + auto common_context = static_cast(context->impl->common); + common_context->thread_is_running.exchange(false); + rmw_ret_t rmw_ret = rmw_fastrtps_shared_cpp::__rmw_trigger_guard_condition( + context->implementation_identifier, common_context->listener_thread_gc); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + try { + common_context->listener_thread.join(); + } catch (const std::exception & exc) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Failed to join std::thread: %s", exc.what()); + return RMW_RET_ERROR; + } catch (...) { + RMW_SET_ERROR_MSG("Failed to join std::thread"); + return RMW_RET_ERROR; + } + rmw_ret = rmw_fastrtps_shared_cpp::__rmw_destroy_guard_condition( + common_context->listener_thread_gc); + if (RMW_RET_OK != rmw_ret) { + return rmw_ret; + } + return RMW_RET_OK; +} + +#define TERMINATE_THREAD(msg) \ + { \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + RCUTILS_STRINGIFY(__FILE__) ":" RCUTILS_STRINGIFY(__function__) ":" \ + RCUTILS_STRINGIFY(__LINE__) RCUTILS_STRINGIFY(msg) \ + ": ros discovery info listener thread will shutdown ...\n"); \ + break; \ + } + +void +node_listener(rmw_context_t * context) +{ + assert(nullptr != context); + assert(nullptr != context->impl); + assert(nullptr != context->impl->common); + auto common_context = static_cast(context->impl->common); + while (common_context->thread_is_running.load()) { + assert(nullptr != common_context->sub); + assert(nullptr != common_context->sub->data); + void * subscriptions_buffer[] = {common_context->sub->data}; + void * guard_conditions_buffer[] = {common_context->listener_thread_gc->data}; + rmw_subscriptions_t subscriptions; + rmw_guard_conditions_t guard_conditions; + subscriptions.subscriber_count = 1; + subscriptions.subscribers = subscriptions_buffer; + guard_conditions.guard_condition_count = 1; + guard_conditions.guard_conditions = guard_conditions_buffer; + // number of conditions of a subscription is 2 + rmw_wait_set_t * wait_set = rmw_fastrtps_shared_cpp::__rmw_create_wait_set( + context->implementation_identifier, context, 2); + if (nullptr == wait_set) { + TERMINATE_THREAD("failed to create wait set"); + } + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_wait( + context->implementation_identifier, + &subscriptions, + &guard_conditions, + nullptr, + nullptr, + nullptr, + wait_set, + nullptr)) + { + TERMINATE_THREAD("rmw_wait failed"); + } + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_destroy_wait_set( + context->implementation_identifier, wait_set)) + { + TERMINATE_THREAD("failed to destroy wait set"); + } + if (subscriptions_buffer[0]) { + rmw_dds_common::msg::ParticipantEntitiesInfo msg; + bool taken; + if (RMW_RET_OK != rmw_fastrtps_shared_cpp::__rmw_take( + context->implementation_identifier, + common_context->sub, + static_cast(&msg), + &taken, + nullptr)) + { + TERMINATE_THREAD("__rmw_take failed"); + } + if (taken) { + if (std::memcmp( + reinterpret_cast(common_context->gid.data), + reinterpret_cast(&msg.gid.data), + RMW_GID_STORAGE_SIZE) == 0) + { + // ignore local messages + continue; + } + common_context->graph_cache.update_participant_entities(msg); + } + } + } +} diff --git a/src/namespace_prefix.cpp b/src/namespace_prefix.cpp new file mode 100644 index 0000000..d02fd0d --- /dev/null +++ b/src/namespace_prefix.cpp @@ -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. + +#include +#include + +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" + +extern "C" +{ +const char * const ros_topic_prefix = "rt"; +const char * const ros_service_requester_prefix = "rq"; +const char * const ros_service_response_prefix = "rr"; + +const std::vector _ros_prefixes = +{ros_topic_prefix, ros_service_requester_prefix, ros_service_response_prefix}; +} // extern "C" + +/// Returns `name` stripped of `prefix`. +std::string +_resolve_prefix(const std::string & name, const std::string & prefix) +{ + if (name.rfind(prefix + "/", 0) == 0) { + return name.substr(prefix.length()); + } + return ""; +} + +/// Return the ROS specific prefix if it exists, otherwise "". +std::string +_get_ros_prefix_if_exists(const std::string & topic_name) +{ + for (const auto & prefix : _ros_prefixes) { + if (topic_name.rfind(prefix + "/", 0) == 0) { + return prefix; + } + } + return ""; +} + +/// Strip the ROS specific prefix if it exists from the topic name. +std::string +_strip_ros_prefix_if_exists(const std::string & topic_name) +{ + for (const auto & prefix : _ros_prefixes) { + if (topic_name.rfind(prefix + "/", 0) == 0) { + return topic_name.substr(prefix.length()); + } + } + return topic_name; +} + +/// Returns the list of ros prefixes +const std::vector & +_get_all_ros_prefixes() +{ + return _ros_prefixes; +} diff --git a/src/participant.cpp b/src/participant.cpp new file mode 100644 index 0000000..1c9ee26 --- /dev/null +++ b/src/participant.cpp @@ -0,0 +1,361 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "fastdds/dds/core/status/StatusMask.hpp" +#include "fastdds/dds/domain/DomainParticipantFactory.hpp" +#include "fastdds/dds/domain/qos/DomainParticipantQos.hpp" +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/publisher/Publisher.hpp" +#include "fastdds/dds/publisher/qos/PublisherQos.hpp" +#include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastdds/dds/subscriber/Subscriber.hpp" +#include "fastdds/dds/subscriber/qos/SubscriberQos.hpp" +#include "fastdds/rtps/attributes/PropertyPolicy.h" +#include "fastdds/rtps/common/Property.h" +#include "fastdds/rtps/transport/UDPv4TransportDescriptor.h" +#include "fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.h" + +#include "rcpputils/scope_exit.hpp" +#include "rcutils/env.h" +#include "rcutils/filesystem.h" + +#include "rmw/allocators.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/participant.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_security_logging.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" + +#include "rmw_dds_common/security.hpp" + +// Private function to create Participant with QoS +static CustomParticipantInfo * +__create_participant( + const char * identifier, + const eprosima::fastdds::dds::DomainParticipantQos & domainParticipantQos, + bool leave_middleware_default_qos, + publishing_mode_t publishing_mode, + rmw_dds_common::Context * common_context, + size_t domain_id) +{ + CustomParticipantInfo * participant_info = nullptr; + + ///// + // Create Custom Participant + try { + participant_info = new CustomParticipantInfo(); + } catch (std::bad_alloc &) { + RMW_SET_ERROR_MSG("__create_participant failed to allocate CustomParticipantInfo struct"); + return nullptr; + } + // lambda to delete participant info + auto cleanup_participant_info = rcpputils::make_scope_exit( + [participant_info]() { + if (nullptr != participant_info->participant_) { + participant_info->participant_->delete_publisher(participant_info->publisher_); + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( + participant_info->participant_); + } + delete participant_info->listener_; + delete participant_info; + }); + + ///// + // Create Participant listener + try { + participant_info->listener_ = new ParticipantListener( + identifier, common_context); + } catch (std::bad_alloc &) { + RMW_SET_ERROR_MSG("__create_participant failed to allocate participant listener"); + return nullptr; + } + + ///// + // Create Participant + + // As the participant listener is only used for discovery related callbacks, which are + // Fast DDS extensions to the DDS standard DomainParticipantListener interface, an empty + // mask should be used to let child entities handle standard DDS events. + eprosima::fastdds::dds::StatusMask participant_mask = eprosima::fastdds::dds::StatusMask::none(); + + participant_info->participant_ = + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->create_participant( + static_cast(domain_id), domainParticipantQos, + participant_info->listener_, participant_mask); + + if (!participant_info->participant_) { + RMW_SET_ERROR_MSG("__create_participant failed to create participant"); + return nullptr; + } + + ///// + // Set participant info parameters + participant_info->leave_middleware_default_qos = leave_middleware_default_qos; + participant_info->publishing_mode = publishing_mode; + + ///// + // Create Publisher + eprosima::fastdds::dds::PublisherQos publisherQos = + participant_info->participant_->get_default_publisher_qos(); + publisherQos.entity_factory(domainParticipantQos.entity_factory()); + + participant_info->publisher_ = participant_info->participant_->create_publisher(publisherQos); + if (!participant_info->publisher_) { + RMW_SET_ERROR_MSG("__create_participant could not create publisher"); + return nullptr; + } + + ///// + // Create Subscriber + eprosima::fastdds::dds::SubscriberQos subscriberQos = + participant_info->participant_->get_default_subscriber_qos(); + subscriberQos.entity_factory(domainParticipantQos.entity_factory()); + + participant_info->subscriber_ = participant_info->participant_->create_subscriber(subscriberQos); + if (!participant_info->subscriber_) { + RMW_SET_ERROR_MSG("__create_participant could not create subscriber"); + return nullptr; + } + + cleanup_participant_info.cancel(); + + return participant_info; +} + +CustomParticipantInfo * +rmw_fastrtps_shared_cpp::create_participant( + const char * identifier, + size_t domain_id, + const rmw_security_options_t * security_options, + bool localhost_only, + const char * enclave, + rmw_dds_common::Context * common_context) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); + + if (!security_options) { + RMW_SET_ERROR_MSG("security_options is null"); + return nullptr; + } + + // Load default XML profile. + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->load_profiles(); + eprosima::fastdds::dds::DomainParticipantQos domainParticipantQos = + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->get_default_participant_qos(); + + + if (localhost_only) { + // In order to use the interface white list, we need to disable the default transport config + domainParticipantQos.transport().use_builtin_transports = false; + + // Add a UDPv4 transport with only localhost enabled + auto udp_transport = std::make_shared(); + udp_transport->interfaceWhiteList.emplace_back("127.0.0.1"); + domainParticipantQos.transport().user_transports.push_back(udp_transport); + + // Add SHM transport if available + auto shm_transport = std::make_shared(); + domainParticipantQos.transport().user_transports.push_back(shm_transport); + } + + size_t length = snprintf(nullptr, 0, "enclave=%s;", enclave) + 1; + domainParticipantQos.user_data().resize(length); + + int written = snprintf( + reinterpret_cast(domainParticipantQos.user_data().data()), + length, "enclave=%s;", enclave); + if (written < 0 || written > static_cast(length) - 1) { + RMW_SET_ERROR_MSG("failed to populate user_data buffer"); + return nullptr; + } + domainParticipantQos.name(enclave); + + bool leave_middleware_default_qos = false; + publishing_mode_t publishing_mode = publishing_mode_t::SYNCHRONOUS; + const char * env_value; + const char * error_str; + error_str = rcutils_get_env("RMW_FASTRTPS_USE_QOS_FROM_XML", &env_value); + if (error_str != NULL) { + RCUTILS_LOG_DEBUG_NAMED("rmw_fastrtps_shared_cpp", "Error getting env var: %s\n", error_str); + return nullptr; + } + if (env_value != nullptr) { + leave_middleware_default_qos = strcmp(env_value, "1") == 0; + } + if (!leave_middleware_default_qos) { + error_str = rcutils_get_env("RMW_FASTRTPS_PUBLICATION_MODE", &env_value); + if (error_str != NULL) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("Error getting env var: %s\n", error_str); + return nullptr; + } + if (env_value != nullptr) { + if (strcmp(env_value, "SYNCHRONOUS") == 0) { + publishing_mode = publishing_mode_t::SYNCHRONOUS; + } else if (strcmp(env_value, "ASYNCHRONOUS") == 0) { + publishing_mode = publishing_mode_t::ASYNCHRONOUS; + } else if (strcmp(env_value, "AUTO") == 0) { + publishing_mode = publishing_mode_t::AUTO; + } else if (strcmp(env_value, "") != 0) { + RCUTILS_LOG_WARN_NAMED( + "rmw_fastrtps_shared_cpp", + "Value %s unknown for environment variable RMW_FASTRTPS_PUBLICATION_MODE" + ". Using default SYNCHRONOUS publishing mode.", env_value); + } + } + } + // allow reallocation to support discovery messages bigger than 5000 bytes + if (!leave_middleware_default_qos) { + domainParticipantQos.wire_protocol().builtin.readerHistoryMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + domainParticipantQos.wire_protocol().builtin.writerHistoryMemoryPolicy = + eprosima::fastrtps::rtps::PREALLOCATED_WITH_REALLOC_MEMORY_MODE; + } + if (security_options->security_root_path) { + // if security_root_path provided, try to find the key and certificate files +#if HAVE_SECURITY + std::unordered_map security_files_paths; + if (rmw_dds_common::get_security_files( + "file://", security_options->security_root_path, security_files_paths)) + { + eprosima::fastrtps::rtps::PropertyPolicy property_policy; + property_policy.properties().emplace_back( + "dds.sec.auth.plugin", "builtin.PKI-DH"); + property_policy.properties().emplace_back( + "dds.sec.auth.builtin.PKI-DH.identity_ca", security_files_paths["IDENTITY_CA"]); + property_policy.properties().emplace_back( + "dds.sec.auth.builtin.PKI-DH.identity_certificate", security_files_paths["CERTIFICATE"]); + property_policy.properties().emplace_back( + "dds.sec.auth.builtin.PKI-DH.private_key", security_files_paths["PRIVATE_KEY"]); + property_policy.properties().emplace_back( + "dds.sec.crypto.plugin", "builtin.AES-GCM-GMAC"); + + property_policy.properties().emplace_back( + "dds.sec.access.plugin", "builtin.Access-Permissions"); + property_policy.properties().emplace_back( + "dds.sec.access.builtin.Access-Permissions.permissions_ca", + security_files_paths["PERMISSIONS_CA"]); + property_policy.properties().emplace_back( + "dds.sec.access.builtin.Access-Permissions.governance", + security_files_paths["GOVERNANCE"]); + property_policy.properties().emplace_back( + "dds.sec.access.builtin.Access-Permissions.permissions", + security_files_paths["PERMISSIONS"]); + + if (security_files_paths.count("CRL") > 0) { + property_policy.properties().emplace_back( + "dds.sec.auth.builtin.PKI-DH.identity_crl", security_files_paths["CRL"]); + } + + // Configure security logging + if (!apply_security_logging_configuration(property_policy)) { + return nullptr; + } + + domainParticipantQos.properties(property_policy); + } else if (security_options->enforce_security) { + RMW_SET_ERROR_MSG("couldn't find all security files!"); + return nullptr; + } +#else + RMW_SET_ERROR_MSG( + "This Fast DDS version doesn't have the security libraries\n" + "Please compile Fast DDS using the -DSECURITY=ON CMake option"); + return nullptr; +#endif + } + return __create_participant( + identifier, + domainParticipantQos, + leave_middleware_default_qos, + publishing_mode, + common_context, + domain_id); +} + +rmw_ret_t +rmw_fastrtps_shared_cpp::destroy_participant(CustomParticipantInfo * participant_info) +{ + if (!participant_info) { + RMW_SET_ERROR_MSG("participant_info is null on destroy_participant"); + return RMW_RET_ERROR; + } + + // Make the participant stop listening to discovery + participant_info->participant_->set_listener(nullptr); + + ReturnCode_t ret = ReturnCode_t::RETCODE_OK; + + // Collect topics that should be deleted + std::vector topics_to_remove; + + // Remove datawriters and publisher from participant + { + std::vector writers; + participant_info->publisher_->get_datawriters(writers); + for (auto writer : writers) { + topics_to_remove.push_back(writer->get_topic()); + participant_info->publisher_->delete_datawriter(writer); + } + ret = participant_info->participant_->delete_publisher(participant_info->publisher_); + if (ReturnCode_t::RETCODE_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to delete dds publisher from participant"); + } + } + + // Remove datareaders and subscriber from participant + { + std::vector readers; + participant_info->subscriber_->get_datareaders(readers); + for (auto reader : readers) { + topics_to_remove.push_back(reader->get_topicdescription()); + participant_info->subscriber_->delete_datareader(reader); + } + ret = participant_info->participant_->delete_subscriber(participant_info->subscriber_); + if (ReturnCode_t::RETCODE_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to delete dds subscriber from participant"); + } + } + + // Remove topics + eprosima::fastdds::dds::TypeSupport dummy_type; + for (auto topic : topics_to_remove) { + remove_topic_and_type(participant_info, topic, dummy_type); + } + + // Delete Domain Participant + ret = + eprosima::fastdds::dds::DomainParticipantFactory::get_instance()->delete_participant( + participant_info->participant_); + + if (ReturnCode_t::RETCODE_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to delete participant"); + } + + // Delete Listener + delete participant_info->listener_; + + // Delete Custom Participant + delete participant_info; + + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + + return RMW_RET_OK; +} diff --git a/src/publisher.cpp b/src/publisher.cpp new file mode 100644 index 0000000..b0a97c8 --- /dev/null +++ b/src/publisher.cpp @@ -0,0 +1,70 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/publisher.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +destroy_publisher( + const char * identifier, + CustomParticipantInfo * participant_info, + rmw_publisher_t * publisher) +{ + assert(publisher->implementation_identifier == identifier); + static_cast(identifier); + + { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Get RMW Publisher + auto info = static_cast(publisher->data); + + // Keep pointer to topic, so we can remove it later + auto topic = info->data_writer_->get_topic(); + + // Delete DataWriter + ReturnCode_t ret = participant_info->publisher_->delete_datawriter(info->data_writer_); + if (ReturnCode_t::RETCODE_OK != ret) { + RMW_SET_ERROR_MSG("Failed to delete datawriter"); + // This is the first failure on this function, and we have not changed state. + // This means it should be safe to return an error + return RMW_RET_ERROR; + } + + // Delete DataWriter listener + delete info->listener_; + + // Delete topic and unregister type + remove_topic_and_type(participant_info, topic, info->type_support_); + + // Delete CustomPublisherInfo structure + delete info; + } + + rmw_free(const_cast(publisher->topic_name)); + rmw_publisher_free(publisher); + + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/qos.cpp b/src/qos.cpp new file mode 100644 index 0000000..74464b7 --- /dev/null +++ b/src/qos.cpp @@ -0,0 +1,214 @@ +// 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_fastrtps_shared_cpp/qos.hpp" + +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" +#include "fastdds/dds/topic/qos/TopicQos.hpp" + +#include "rmw/error_handling.h" + +#include "time_utils.hpp" + +static +bool +is_rmw_duration_unspecified(const rmw_time_t & time) +{ + return rmw_time_equal(time, RMW_DURATION_UNSPECIFIED); +} + +rmw_time_t +dds_duration_to_rmw(const eprosima::fastrtps::Duration_t & duration) +{ + if (duration == eprosima::fastrtps::rtps::c_RTPSTimeInfinite) { + return RMW_DURATION_INFINITE; + } + rmw_time_t result = {(uint64_t)duration.seconds, (uint64_t)duration.nanosec}; + return result; +} + +// Private function to encapsulate DataReader and DataWriter, together with Topic, filling +// entities DDS QoS from the RMW QoS profile. +template +bool fill_entity_qos_from_profile( + const rmw_qos_profile_t & qos_policies, + DDSEntityQos & entity_qos) +{ + switch (qos_policies.history) { + case RMW_QOS_POLICY_HISTORY_KEEP_LAST: + entity_qos.history().kind = eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS; + break; + case RMW_QOS_POLICY_HISTORY_KEEP_ALL: + entity_qos.history().kind = eprosima::fastdds::dds::KEEP_ALL_HISTORY_QOS; + break; + case RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS history policy"); + return false; + } + + switch (qos_policies.durability) { + case RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL: + entity_qos.durability().kind = eprosima::fastdds::dds::TRANSIENT_LOCAL_DURABILITY_QOS; + break; + case RMW_QOS_POLICY_DURABILITY_VOLATILE: + entity_qos.durability().kind = eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS; + break; + case RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS durability policy"); + return false; + } + + switch (qos_policies.reliability) { + case RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT: + entity_qos.reliability().kind = eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS; + break; + case RMW_QOS_POLICY_RELIABILITY_RELIABLE: + entity_qos.reliability().kind = eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS; + break; + case RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS reliability policy"); + return false; + } + + // ensure the history depth is at least the requested queue size + assert(entity_qos.history().depth >= 0); + if ( + qos_policies.depth != RMW_QOS_POLICY_DEPTH_SYSTEM_DEFAULT && + static_cast(entity_qos.history().depth) < qos_policies.depth) + { + if (qos_policies.depth > static_cast((std::numeric_limits::max)())) { + RMW_SET_ERROR_MSG( + "failed to set history depth since the requested queue size exceeds the DDS type"); + return false; + } + entity_qos.history().depth = static_cast(qos_policies.depth); + } + + if (!is_rmw_duration_unspecified(qos_policies.lifespan)) { + entity_qos.lifespan().duration = + rmw_fastrtps_shared_cpp::rmw_time_to_fastrtps(qos_policies.lifespan); + } + + if (!is_rmw_duration_unspecified(qos_policies.deadline)) { + entity_qos.deadline().period = + rmw_fastrtps_shared_cpp::rmw_time_to_fastrtps(qos_policies.deadline); + } + + switch (qos_policies.liveliness) { + case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC: + entity_qos.liveliness().kind = eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS; + break; + case RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC: + entity_qos.liveliness().kind = eprosima::fastdds::dds::MANUAL_BY_TOPIC_LIVELINESS_QOS; + break; + case RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT: + break; + default: + RMW_SET_ERROR_MSG("Unknown QoS Liveliness policy"); + return false; + } + if (!is_rmw_duration_unspecified(qos_policies.liveliness_lease_duration)) { + entity_qos.liveliness().lease_duration = + rmw_fastrtps_shared_cpp::rmw_time_to_fastrtps(qos_policies.liveliness_lease_duration); + + // Docs suggest setting no higher than 0.7 * lease_duration, choosing 2/3 to give safe buffer. + // See doc at https://github.com/eProsima/Fast-RTPS/blob/ + // a8691a40be6b8460b01edde36ad8563170a3a35a/include/fastrtps/qos/QosPolicies.h#L223-L232 + double period_in_ns = entity_qos.liveliness().lease_duration.to_ns() * 2.0 / 3.0; + double period_in_s = RCUTILS_NS_TO_S(period_in_ns); + entity_qos.liveliness().announcement_period = eprosima::fastrtps::Duration_t(period_in_s); + } + + return true; +} + +bool +get_datareader_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastdds::dds::DataReaderQos & datareader_qos) +{ + return fill_entity_qos_from_profile(qos_policies, datareader_qos); +} + +bool +get_datawriter_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastdds::dds::DataWriterQos & datawriter_qos) +{ + return fill_entity_qos_from_profile(qos_policies, datawriter_qos); +} + +bool +get_topic_qos( + const rmw_qos_profile_t & qos_policies, + eprosima::fastdds::dds::TopicQos & topic_qos) +{ + return fill_entity_qos_from_profile(qos_policies, topic_qos); +} + +bool +is_valid_qos(const rmw_qos_profile_t & /* qos_policies */) +{ + return true; +} + +template +void +dds_attributes_to_rmw_qos( + const AttributeT & dds_qos, + rmw_qos_profile_t * qos) +{ + switch (dds_qos.topic.historyQos.kind) { + case eprosima::fastrtps::KEEP_LAST_HISTORY_QOS: + qos->history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + break; + case eprosima::fastrtps::KEEP_ALL_HISTORY_QOS: + qos->history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + break; + default: + qos->history = RMW_QOS_POLICY_HISTORY_UNKNOWN; + break; + } + qos->depth = static_cast(dds_qos.topic.historyQos.depth); + rtps_qos_to_rmw_qos(dds_qos.qos, qos); +} + +template +void dds_attributes_to_rmw_qos( + const eprosima::fastrtps::PublisherAttributes & dds_qos, + rmw_qos_profile_t * qos); + +template +void dds_attributes_to_rmw_qos( + const eprosima::fastrtps::SubscriberAttributes & dds_qos, + rmw_qos_profile_t * qos); + +template +void dds_qos_to_rmw_qos( + const eprosima::fastdds::dds::DataWriterQos & dds_qos, + rmw_qos_profile_t * qos); + +template +void dds_qos_to_rmw_qos( + const eprosima::fastdds::dds::DataReaderQos & dds_qos, + rmw_qos_profile_t * qos); diff --git a/src/rmw_client.cpp b/src/rmw_client.cpp new file mode 100644 index 0000000..dfeb174 --- /dev/null +++ b/src/rmw_client.cpp @@ -0,0 +1,164 @@ +// 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/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/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_destroy_client( + const char * identifier, + rmw_node_t * node, + rmw_client_t * client) +{ + rmw_ret_t final_ret = RMW_RET_OK; + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + auto info = static_cast(client->data); + + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->request_writer_->guid()); + common_context->graph_cache.dissociate_writer( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->response_reader_->guid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( + gid, common_context->gid, node->name, node->namespace_); + final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + } + + auto show_previous_error = + [&final_ret]() { + if (RMW_RET_OK != final_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + rmw_reset_error(); + } + }; + + ///// + // Delete DataWriter and DataReader + { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Keep pointers to topics, so we can remove them later + auto response_topic = info->response_reader_->get_topicdescription(); + auto request_topic = info->request_writer_->get_topic(); + + // Delete DataReader + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->response_reader_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("destroy_client() failed to delete datareader"); + final_ret = RMW_RET_ERROR; + info->response_reader_->set_listener(nullptr); + } + + // Delete DataReader listener + if (nullptr != info->listener_) { + delete info->listener_; + } + + // Delete DataWriter + ret = participant_info->publisher_->delete_datawriter(info->request_writer_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("destroy_client() failed to delete datawriter"); + final_ret = RMW_RET_ERROR; + info->request_writer_->set_listener(nullptr); + } + + // Delete DataWriter listener + if (nullptr != info->pub_listener_) { + delete info->pub_listener_; + } + + // Delete topics and unregister types + remove_topic_and_type(participant_info, request_topic, info->request_type_support_); + remove_topic_and_type(participant_info, response_topic, info->response_type_support_); + + // Delete CustomClientInfo structure + delete info; + } + + rmw_free(const_cast(client->service_name)); + rmw_client_free(client); + + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + return final_ret; +} + +rmw_ret_t +__rmw_client_request_publisher_get_actual_qos( + const rmw_client_t * client, + rmw_qos_profile_t * qos) +{ + auto cli = static_cast(client->data); + eprosima::fastdds::dds::DataWriter * fastdds_rw = cli->request_writer_; + dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_client_response_subscription_get_actual_qos( + const rmw_client_t * client, + rmw_qos_profile_t * qos) +{ + auto cli = static_cast(client->data); + eprosima::fastdds::dds::DataReader * fastdds_dr = cli->response_reader_; + dds_qos_to_rmw_qos(fastdds_dr->get_qos(), qos); + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_client_set_on_new_response_callback( + rmw_client_t * rmw_client, + rmw_event_callback_t callback, + const void * user_data) +{ + auto custom_client_info = static_cast(rmw_client->data); + custom_client_info->listener_->set_on_new_response_callback( + user_data, + callback); + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_compare_gids_equal.cpp b/src/rmw_compare_gids_equal.cpp new file mode 100644 index 0000000..2a902a6 --- /dev/null +++ b/src/rmw_compare_gids_equal.cpp @@ -0,0 +1,52 @@ +// 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 "fastdds/rtps/common/Guid.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" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_compare_gids_equal( + const char * identifier, + const rmw_gid_t * gid1, + const rmw_gid_t * gid2, + bool * result) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(gid1, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + gid1, + gid1->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(gid2, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + gid2, + gid2->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(result, RMW_RET_INVALID_ARGUMENT); + + *result = + memcmp(gid1->data, gid2->data, sizeof(eprosima::fastrtps::rtps::GUID_t)) == 0; + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_count.cpp b/src/rmw_count.cpp new file mode 100644 index 0000000..59b5bd9 --- /dev/null +++ b/src/rmw_count.cpp @@ -0,0 +1,100 @@ +// 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 "rcutils/logging_macros.h" + +#include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/rmw.h" +#include "rmw/types.h" +#include "rmw/validate_full_topic_name.h" + +#include "rmw_dds_common/context.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +rmw_ret_t +__rmw_count_publishers( + const char * identifier, + const rmw_node_t * node, + const char * topic_name, + size_t * count) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_INVALID_ARGUMENT); + 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 ret; + } + 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("topic_name argument is invalid: %s", reason); + return RMW_RET_INVALID_ARGUMENT; + } + RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); + auto common_context = static_cast(node->context->impl->common); + const std::string mangled_topic_name = + _mangle_topic_name(ros_topic_prefix, topic_name).to_string(); + return common_context->graph_cache.get_writer_count(mangled_topic_name, count); +} + +rmw_ret_t +__rmw_count_subscribers( + const char * identifier, + const rmw_node_t * node, + const char * topic_name, + size_t * count) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_INVALID_ARGUMENT); + 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 ret; + } + 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("topic_name argument is invalid: %s", reason); + return RMW_RET_INVALID_ARGUMENT; + } + RMW_CHECK_ARGUMENT_FOR_NULL(count, RMW_RET_INVALID_ARGUMENT); + auto common_context = static_cast(node->context->impl->common); + const std::string mangled_topic_name = + _mangle_topic_name(ros_topic_prefix, topic_name).to_string(); + return common_context->graph_cache.get_reader_count(mangled_topic_name, count); +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_event.cpp b/src/rmw_event.cpp new file mode 100644 index 0000000..ccc5375 --- /dev/null +++ b/src/rmw_event.cpp @@ -0,0 +1,111 @@ +// 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 + +#include "rmw/impl/cpp/macros.hpp" + +#include "event_helpers.hpp" +#include "rmw_fastrtps_shared_cpp/custom_event_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "types/event_types.hpp" + +static const std::unordered_set g_rmw_event_type_set{ + RMW_EVENT_LIVELINESS_CHANGED, + RMW_EVENT_REQUESTED_DEADLINE_MISSED, + RMW_EVENT_LIVELINESS_LOST, + RMW_EVENT_OFFERED_DEADLINE_MISSED, + RMW_EVENT_MESSAGE_LOST, + RMW_EVENT_OFFERED_QOS_INCOMPATIBLE, + RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE +}; + +namespace rmw_fastrtps_shared_cpp +{ +namespace internal +{ + +bool is_event_supported(rmw_event_type_t event_type) +{ + return g_rmw_event_type_set.count(event_type) == 1; +} + +rmw_qos_policy_kind_t dds_qos_policy_to_rmw_qos_policy( + eprosima::fastdds::dds::QosPolicyId_t policy_id) +{ + using eprosima::fastdds::dds::QosPolicyId_t; + + switch (policy_id) { + case QosPolicyId_t::DURABILITY_QOS_POLICY_ID: + return RMW_QOS_POLICY_DURABILITY; + case QosPolicyId_t::DEADLINE_QOS_POLICY_ID: + return RMW_QOS_POLICY_DEADLINE; + case QosPolicyId_t::LIVELINESS_QOS_POLICY_ID: + return RMW_QOS_POLICY_LIVELINESS; + case QosPolicyId_t::RELIABILITY_QOS_POLICY_ID: + return RMW_QOS_POLICY_RELIABILITY; + case QosPolicyId_t::HISTORY_QOS_POLICY_ID: + return RMW_QOS_POLICY_HISTORY; + case QosPolicyId_t::LIFESPAN_QOS_POLICY_ID: + return RMW_QOS_POLICY_LIFESPAN; + default: + return RMW_QOS_POLICY_INVALID; + } +} + +} // namespace internal + +rmw_ret_t +__rmw_init_event( + const char * identifier, + rmw_event_t * rmw_event, + const char * topic_endpoint_impl_identifier, + void * data, + rmw_event_type_t event_type) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(identifier, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(rmw_event, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_endpoint_impl_identifier, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(data, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + topic endpoint, + topic_endpoint_impl_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!internal::is_event_supported(event_type)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("provided event_type is not supported by %s", identifier); + return RMW_RET_UNSUPPORTED; + } + + rmw_event->implementation_identifier = topic_endpoint_impl_identifier; + rmw_event->data = data; + rmw_event->event_type = event_type; + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_event_set_callback( + rmw_event_t * rmw_event, + rmw_event_callback_t callback, + const void * user_data) +{ + auto custom_event_info = static_cast(rmw_event->data); + custom_event_info->getListener()->set_on_new_event_callback( + user_data, + callback); + return RMW_RET_OK; +} + +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_features.cpp b/src/rmw_features.cpp new file mode 100644 index 0000000..5296c58 --- /dev/null +++ b/src/rmw_features.cpp @@ -0,0 +1,26 @@ +// 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" + +bool +rmw_fastrtps_shared_cpp::__rmw_feature_supported(rmw_feature_t feature) +{ + if (feature == RMW_FEATURE_MESSAGE_INFO_PUBLICATION_SEQUENCE_NUMBER) { + return true; + } + return false; +} diff --git a/src/rmw_get_endpoint_network_flow.cpp b/src/rmw_get_endpoint_network_flow.cpp new file mode 100644 index 0000000..73e6d37 --- /dev/null +++ b/src/rmw_get_endpoint_network_flow.cpp @@ -0,0 +1,204 @@ +// 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 + +#include "rmw/get_network_flow_endpoints.h" +#include "rmw/error_handling.h" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "fastrtps/utils/IPLocator.h" + + +namespace rmw_fastrtps_shared_cpp +{ + +using Locator_t = eprosima::fastrtps::rtps::Locator_t; +using LocatorList_t = eprosima::fastrtps::rtps::LocatorList_t; +using IPLocator = eprosima::fastrtps::rtps::IPLocator; + +rmw_ret_t fill_network_flow_endpoint(rmw_network_flow_endpoint_t *, const Locator_t &); + +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) +{ + rmw_ret_t res = RMW_RET_OK; + + // Retrieve the sender locators + CustomPublisherInfo * data = + static_cast(publisher->data); + LocatorList_t locators; + data->data_writer_->get_sending_locators(locators); + + if (locators.empty()) { + return res; + } + + // It must be a non-initialized array + if (RMW_RET_OK != + (res = rmw_network_flow_endpoint_array_check_zero(network_flow_endpoint_array))) + { + return res; + } + + // Allocate + if (RMW_RET_OK != + (res = rmw_network_flow_endpoint_array_init( + network_flow_endpoint_array, + locators.size(), + allocator))) + { + return res; + } + + // Translate the locators, on error reset the array + try { + auto rmw_nf = network_flow_endpoint_array->network_flow_endpoint; + for (const Locator_t & loc : locators) { + if (RMW_RET_OK != + (res = fill_network_flow_endpoint(rmw_nf++, loc))) + { + throw res; + } + } + } catch (rmw_ret_t) { + // clear the array + rmw_network_flow_endpoint_array_fini( + network_flow_endpoint_array); + + // set error message + RMW_SET_ERROR_MSG("Failed to compose network_flow_endpoint_array"); + } + + return res; +} + +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) +{ + rmw_ret_t res = RMW_RET_OK; + + // Retrieve the listener locators + CustomSubscriberInfo * data = + static_cast(subscription->data); + LocatorList_t locators; + data->data_reader_->get_listening_locators(locators); + + if (locators.empty()) { + return res; + } + + // It must be a non-initialized array + if (RMW_RET_OK != + (res = rmw_network_flow_endpoint_array_check_zero(network_flow_endpoint_array))) + { + return res; + } + + // Allocate + if (RMW_RET_OK != + (res = rmw_network_flow_endpoint_array_init( + network_flow_endpoint_array, + locators.size(), + allocator))) + { + return res; + } + + // Translate the locators, on error reset the array + try { + auto rmw_nf = network_flow_endpoint_array->network_flow_endpoint; + for (const Locator_t & loc : locators) { + if (RMW_RET_OK != + (res = fill_network_flow_endpoint(rmw_nf++, loc))) + { + throw res; + } + } + } catch (rmw_ret_t) { + // clear the array + rmw_network_flow_endpoint_array_fini( + network_flow_endpoint_array); + + // set error message + RMW_SET_ERROR_MSG("Failed to compose network_flow_endpoint_array"); + } + + return res; +} + +// Ancillary translation methods +rmw_transport_protocol_t +get_transport_protocol(const Locator_t & loc) +{ + if (loc.kind & (LOCATOR_KIND_UDPv4 | LOCATOR_KIND_UDPv6)) { + return RMW_TRANSPORT_PROTOCOL_UDP; + } else if (loc.kind & (LOCATOR_KIND_TCPv4 | LOCATOR_KIND_TCPv6)) { + return RMW_TRANSPORT_PROTOCOL_TCP; + } + + return RMW_TRANSPORT_PROTOCOL_UNKNOWN; +} + +rmw_internet_protocol_t +get_internet_protocol(const Locator_t & loc) +{ + if (loc.kind & (LOCATOR_KIND_UDPv4 | LOCATOR_KIND_TCPv4)) { + return RMW_INTERNET_PROTOCOL_IPV4; + } else if (loc.kind & (LOCATOR_KIND_TCPv6 | LOCATOR_KIND_UDPv6)) { + return RMW_INTERNET_PROTOCOL_IPV6; + } + + return RMW_INTERNET_PROTOCOL_UNKNOWN; +} + +rmw_ret_t +fill_network_flow_endpoint( + rmw_network_flow_endpoint_t * network_flow_endpoint, + const Locator_t & locator) +{ + rmw_ret_t res = RMW_RET_OK; + + // Translate transport protocol + network_flow_endpoint->transport_protocol = get_transport_protocol(locator); + + // Translate internet protocol + network_flow_endpoint->internet_protocol = get_internet_protocol(locator); + + // Set the port + network_flow_endpoint->transport_port = IPLocator::getPhysicalPort(locator); + + // Set the address + std::string address = IPLocator::ip_to_string(locator); + + if (RMW_RET_OK != + (res = rmw_network_flow_endpoint_set_internet_address( + network_flow_endpoint, + address.c_str(), + address.length()))) + { + return res; + } + + return res; +} + +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_get_gid_for_publisher.cpp b/src/rmw_get_gid_for_publisher.cpp new file mode 100644 index 0000000..83cd1f1 --- /dev/null +++ b/src/rmw_get_gid_for_publisher.cpp @@ -0,0 +1,43 @@ +// 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/types.h" + +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_get_gid_for_publisher( + const char * identifier, + const rmw_publisher_t * publisher, + rmw_gid_t * gid) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, + publisher->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(gid, RMW_RET_INVALID_ARGUMENT); + + auto info = static_cast(publisher->data); + *gid = info->publisher_gid; + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_get_topic_endpoint_info.cpp b/src/rmw_get_topic_endpoint_info.cpp new file mode 100644 index 0000000..45d4b8e --- /dev/null +++ b/src/rmw_get_topic_endpoint_info.cpp @@ -0,0 +1,126 @@ +// Copyright 2019 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/rmw.h" +#include "rmw/types.h" +#include "rmw/topic_endpoint_info_array.h" +#include "rmw/topic_endpoint_info.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +#include "rmw_dds_common/graph_cache.hpp" + +#include "demangle.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +static rmw_ret_t __validate_arguments( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + rmw_topic_endpoint_info_array_t * participants_info) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "allocator argument is invalid", return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(topic_name, RMW_RET_INVALID_ARGUMENT); + if (RMW_RET_OK != rmw_topic_endpoint_info_array_check_zero(participants_info)) { + return RMW_RET_INVALID_ARGUMENT; + } + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_get_publishers_info_by_topic( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * publishers_info) +{ + rmw_ret_t ret = __validate_arguments( + identifier, + node, + allocator, + topic_name, + publishers_info); + if (ret != RMW_RET_OK) { + return ret; + } + auto common_context = static_cast(node->context->impl->common); + std::string mangled_topic_name = topic_name; + DemangleFunction demangle_type = _identity_demangle; + if (!no_mangle) { + mangled_topic_name = _mangle_topic_name(ros_topic_prefix, topic_name).to_string(); + demangle_type = _demangle_if_ros_type; + } + + return common_context->graph_cache.get_writers_info_by_topic( + mangled_topic_name, + demangle_type, + allocator, + publishers_info); +} + +rmw_ret_t +__rmw_get_subscriptions_info_by_topic( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * subscriptions_info) +{ + rmw_ret_t ret = __validate_arguments( + identifier, + node, + allocator, + topic_name, + subscriptions_info); + if (ret != RMW_RET_OK) { + return ret; + } + auto common_context = static_cast(node->context->impl->common); + std::string mangled_topic_name = topic_name; + DemangleFunction demangle_type = _identity_demangle; + if (!no_mangle) { + mangled_topic_name = _mangle_topic_name(ros_topic_prefix, topic_name).to_string(); + demangle_type = _demangle_if_ros_type; + } + + return common_context->graph_cache.get_readers_info_by_topic( + mangled_topic_name, + demangle_type, + allocator, + subscriptions_info); +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_guard_condition.cpp b/src/rmw_guard_condition.cpp new file mode 100644 index 0000000..cdcac52 --- /dev/null +++ b/src/rmw_guard_condition.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/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "types/guard_condition.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_guard_condition_t * +__rmw_create_guard_condition(const char * identifier) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); + + rmw_guard_condition_t * guard_condition_handle = new rmw_guard_condition_t; + guard_condition_handle->implementation_identifier = identifier; + guard_condition_handle->data = new GuardCondition(); + return guard_condition_handle; +} + +rmw_ret_t +__rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition) +{ + rmw_ret_t ret = RMW_RET_ERROR; + + if (guard_condition) { + delete static_cast(guard_condition->data); + delete guard_condition; + ret = RMW_RET_OK; + } + + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + return ret; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_init.cpp b/src/rmw_init.cpp new file mode 100644 index 0000000..830dcc6 --- /dev/null +++ b/src/rmw_init.cpp @@ -0,0 +1,112 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw_fastrtps_shared_cpp/rmw_init.hpp" + +#include "rcutils/strdup.h" +#include "rcutils/types.h" + +#include "rmw/init.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/init_options.h" + +namespace rmw_fastrtps_shared_cpp +{ + +rmw_ret_t +rmw_init_options_init( + const char * identifier, rmw_init_options_t * init_options, rcutils_allocator_t allocator) +{ + assert(identifier != NULL); + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + RCUTILS_CHECK_ALLOCATOR(&allocator, return RMW_RET_INVALID_ARGUMENT); + if (NULL != init_options->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized init_options"); + return RMW_RET_INVALID_ARGUMENT; + } + init_options->instance_id = 0; + init_options->implementation_identifier = identifier; + init_options->allocator = allocator; + init_options->impl = nullptr; + init_options->enclave = NULL; + init_options->domain_id = RMW_DEFAULT_DOMAIN_ID; + init_options->security_options = rmw_get_default_security_options(); + init_options->localhost_only = RMW_LOCALHOST_ONLY_DEFAULT; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_init_options_copy( + const char * identifier, const rmw_init_options_t * src, rmw_init_options_t * dst) +{ + assert(identifier != NULL); + RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT); + if (NULL == src->implementation_identifier) { + RMW_SET_ERROR_MSG("expected initialized dst"); + return RMW_RET_INVALID_ARGUMENT; + } + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + src, + src->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (NULL != dst->implementation_identifier) { + RMW_SET_ERROR_MSG("expected zero-initialized dst"); + return RMW_RET_INVALID_ARGUMENT; + } + const rcutils_allocator_t * allocator = &src->allocator; + RCUTILS_CHECK_ALLOCATOR(allocator, return RMW_RET_INVALID_ARGUMENT); + + rmw_init_options_t tmp = *src; + tmp.enclave = rcutils_strdup(tmp.enclave, *allocator); + if (NULL != src->enclave && NULL == tmp.enclave) { + return RMW_RET_BAD_ALLOC; + } + tmp.security_options = rmw_get_zero_initialized_security_options(); + rmw_ret_t ret = + rmw_security_options_copy(&src->security_options, allocator, &tmp.security_options); + if (RMW_RET_OK != ret) { + allocator->deallocate(tmp.enclave, allocator->state); + return ret; + } + *dst = tmp; + return RMW_RET_OK; +} + +rmw_ret_t +rmw_init_options_fini(const char * identifier, rmw_init_options_t * init_options) +{ + assert(identifier != NULL); + RMW_CHECK_ARGUMENT_FOR_NULL(init_options, RMW_RET_INVALID_ARGUMENT); + if (NULL == init_options->implementation_identifier) { + RMW_SET_ERROR_MSG("expected initialized init_options"); + return RMW_RET_INVALID_ARGUMENT; + } + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + init_options, + init_options->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + rcutils_allocator_t * allocator = &init_options->allocator; + RCUTILS_CHECK_ALLOCATOR(allocator, return RMW_RET_INVALID_ARGUMENT); + + allocator->deallocate(init_options->enclave, allocator->state); + rmw_ret_t ret = rmw_security_options_fini(&init_options->security_options, allocator); + *init_options = rmw_get_zero_initialized_init_options(); + return ret; +} + +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_logging.cpp b/src/rmw_logging.cpp new file mode 100644 index 0000000..db6b86a --- /dev/null +++ b/src/rmw_logging.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 "rmw/rmw.h" +#include "rmw/error_handling.h" + +#include "rcutils/logging_macros.h" + +#include "fastdds/dds/log/Log.hpp" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +rmw_ret_t +__rmw_set_log_severity(rmw_log_severity_t severity) +{ + using eprosima::fastdds::dds::Log; + + Log::Kind log_kind; + + switch (severity) { + case RMW_LOG_SEVERITY_DEBUG: +// fall through + case RMW_LOG_SEVERITY_INFO: + log_kind = Log::Kind::Info; + break; + case RMW_LOG_SEVERITY_WARN: + log_kind = Log::Kind::Warning; + break; + case RMW_LOG_SEVERITY_ERROR: +// fall through + case RMW_LOG_SEVERITY_FATAL: + log_kind = Log::Kind::Error; + break; + default: + RCUTILS_LOG_ERROR("Unknown logging severity type %d", severity); + return RMW_RET_ERROR; + } + + Log::SetVerbosity(log_kind); + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_node.cpp b/src/rmw_node.cpp new file mode 100644 index 0000000..e6d56ef --- /dev/null +++ b/src/rmw_node.cpp @@ -0,0 +1,164 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#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_namespace.h" +#include "rmw/validate_node_name.h" + +#include "rcpputils/scope_exit.hpp" + +#include "rmw_dds_common/context.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + + +namespace rmw_fastrtps_shared_cpp +{ +rmw_node_t * +__rmw_create_node( + rmw_context_t * context, + const char * identifier, + const char * name, + const char * namespace_) +{ + assert(identifier == context->implementation_identifier); + + int validation_result = RMW_NODE_NAME_VALID; + rmw_ret_t ret = rmw_validate_node_name(name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; + } + if (RMW_NODE_NAME_VALID != validation_result) { + const char * reason = rmw_node_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid node name: %s", reason); + return nullptr; + } + validation_result = RMW_NAMESPACE_VALID; + ret = rmw_validate_namespace(namespace_, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return nullptr; + } + if (RMW_NAMESPACE_VALID != validation_result) { + const char * reason = rmw_node_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("invalid node namespace: %s", reason); + return nullptr; + } + + auto common_context = static_cast(context->impl->common); + rmw_dds_common::GraphCache & graph_cache = common_context->graph_cache; + rmw_node_t * node_handle = rmw_node_allocate(); + if (nullptr == node_handle) { + RMW_SET_ERROR_MSG("failed to allocate node"); + return nullptr; + } + auto cleanup_node = rcpputils::make_scope_exit( + [node_handle]() { + rmw_free(const_cast(node_handle->name)); + rmw_free(const_cast(node_handle->namespace_)); + rmw_node_free(node_handle); + }); + node_handle->implementation_identifier = identifier; + node_handle->data = nullptr; + + node_handle->name = + static_cast(rmw_allocate(sizeof(char) * strlen(name) + 1)); + if (nullptr == node_handle->name) { + RMW_SET_ERROR_MSG("failed to copy node name"); + return nullptr; + } + memcpy(const_cast(node_handle->name), name, strlen(name) + 1); + + node_handle->namespace_ = + static_cast(rmw_allocate(sizeof(char) * strlen(namespace_) + 1)); + if (nullptr == node_handle->namespace_) { + RMW_SET_ERROR_MSG("failed to copy node namespace"); + return nullptr; + } + memcpy(const_cast(node_handle->namespace_), namespace_, strlen(namespace_) + 1); + + node_handle->context = context; + + { + // Though graph_cache methods are thread safe, both cache update and publishing have to also + // be atomic. + // If not, the following race condition is possible: + // node1-update-get-message / node2-update-get-message / node2-publish / node1-publish + // In that case, the last message published is not accurate. + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo participant_msg = + graph_cache.add_node(common_context->gid, name, namespace_); + if (RMW_RET_OK != __rmw_publish( + node_handle->implementation_identifier, + common_context->pub, + static_cast(&participant_msg), + nullptr)) + { + return nullptr; + } + } + cleanup_node.cancel(); + return node_handle; +} + +rmw_ret_t +__rmw_destroy_node( + const char * identifier, + rmw_node_t * node) +{ + assert(node->implementation_identifier == identifier); + rmw_ret_t ret = RMW_RET_OK; + auto common_context = static_cast(node->context->impl->common); + rmw_dds_common::GraphCache & graph_cache = common_context->graph_cache; + { + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo participant_msg = + graph_cache.remove_node(common_context->gid, node->name, node->namespace_); + ret = __rmw_publish( + identifier, + common_context->pub, + static_cast(&participant_msg), + nullptr); + } + rmw_free(const_cast(node->name)); + rmw_free(const_cast(node->namespace_)); + rmw_node_free(node); + + return ret; +} + +const rmw_guard_condition_t * +__rmw_node_get_graph_guard_condition(const rmw_node_t * node) +{ + auto common_context = static_cast(node->context->impl->common); + if (!common_context) { + RMW_SET_ERROR_MSG("common_context is nullptr"); + return nullptr; + } + return common_context->graph_guard_condition; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_node_info_and_types.cpp b/src/rmw_node_info_and_types.cpp new file mode 100644 index 0000000..12e90b1 --- /dev/null +++ b/src/rmw_node_info_and_types.cpp @@ -0,0 +1,249 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2018 Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// 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 "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/validate_namespace.h" +#include "rmw/validate_node_name.h" + +#include "rmw_dds_common/context.hpp" + +#include "demangle.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/names.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +using GetNamesAndTypesByNodeFunction = rmw_ret_t (*)( + rmw_dds_common::Context *, + const std::string &, + const std::string &, + DemangleFunction, + DemangleFunction, + rcutils_allocator_t *, + rmw_names_and_types_t *); + +rmw_ret_t +__rmw_get_topic_names_and_types_by_node( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + DemangleFunction demangle_topic, + DemangleFunction demangle_type, + bool no_demangle, + GetNamesAndTypesByNodeFunction get_names_and_types_by_node, + rmw_names_and_types_t * topic_names_and_types) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "allocator argument is invalid", return RMW_RET_INVALID_ARGUMENT); + int validation_result = RMW_NODE_NAME_VALID; + rmw_ret_t ret = rmw_validate_node_name(node_name, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return ret; + } + if (RMW_NODE_NAME_VALID != validation_result) { + const char * reason = rmw_node_name_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("node_name argument is invalid: %s", reason); + return RMW_RET_INVALID_ARGUMENT; + } + validation_result = RMW_NAMESPACE_VALID; + ret = rmw_validate_namespace(node_namespace, &validation_result, nullptr); + if (RMW_RET_OK != ret) { + return ret; + } + if (RMW_NAMESPACE_VALID != validation_result) { + const char * reason = rmw_namespace_validation_result_string(validation_result); + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("node_namespace argument is invalid: %s", reason); + return RMW_RET_INVALID_ARGUMENT; + } + ret = rmw_names_and_types_check_zero(topic_names_and_types); + if (RMW_RET_OK != ret) { + return ret; + } + auto common_context = static_cast(node->context->impl->common); + + if (no_demangle) { + demangle_topic = _identity_demangle; + demangle_type = _identity_demangle; + } + + return get_names_and_types_by_node( + common_context, + node_name, + node_namespace, + demangle_topic, + demangle_type, + allocator, + topic_names_and_types); +} + +rmw_ret_t +__get_reader_names_and_types_by_node( + rmw_dds_common::Context * common_context, + const std::string & node_name, + const std::string & node_namespace, + DemangleFunction demangle_topic, + DemangleFunction demangle_type, + rcutils_allocator_t * allocator, + rmw_names_and_types_t * topic_names_and_types) +{ + return common_context->graph_cache.get_reader_names_and_types_by_node( + node_name, + node_namespace, + demangle_topic, + demangle_type, + allocator, + topic_names_and_types); +} + +rmw_ret_t +__get_writer_names_and_types_by_node( + rmw_dds_common::Context * common_context, + const std::string & node_name, + const std::string & node_namespace, + DemangleFunction demangle_topic, + DemangleFunction demangle_type, + rcutils_allocator_t * allocator, + rmw_names_and_types_t * topic_names_and_types) +{ + return common_context->graph_cache.get_writer_names_and_types_by_node( + node_name, + node_namespace, + demangle_topic, + demangle_type, + allocator, + topic_names_and_types); +} + +rmw_ret_t +__rmw_get_subscriber_names_and_types_by_node( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) +{ + return __rmw_get_topic_names_and_types_by_node( + identifier, + node, + allocator, + node_name, + node_namespace, + _demangle_ros_topic_from_topic, + _demangle_if_ros_type, + no_demangle, + __get_reader_names_and_types_by_node, + topic_names_and_types); +} + +rmw_ret_t +__rmw_get_publisher_names_and_types_by_node( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) +{ + return __rmw_get_topic_names_and_types_by_node( + identifier, + node, + allocator, + node_name, + node_namespace, + _demangle_ros_topic_from_topic, + _demangle_if_ros_type, + no_demangle, + __get_writer_names_and_types_by_node, + topic_names_and_types); +} + +rmw_ret_t +__rmw_get_service_names_and_types_by_node( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rmw_names_and_types_t * service_names_and_types) +{ + return __rmw_get_topic_names_and_types_by_node( + identifier, + node, + allocator, + node_name, + node_namespace, + _demangle_service_request_from_topic, + _demangle_service_type_only, + false, + __get_reader_names_and_types_by_node, + service_names_and_types); +} + +rmw_ret_t +__rmw_get_client_names_and_types_by_node( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rmw_names_and_types_t * service_names_and_types) +{ + return __rmw_get_topic_names_and_types_by_node( + identifier, + node, + allocator, + node_name, + node_namespace, + _demangle_service_reply_from_topic, + _demangle_service_type_only, + false, + __get_reader_names_and_types_by_node, + service_names_and_types); +} + +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_node_names.cpp b/src/rmw_node_names.cpp new file mode 100644 index 0000000..415070e --- /dev/null +++ b/src/rmw_node_names.cpp @@ -0,0 +1,98 @@ +// 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/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/impl/cpp/macros.hpp" +#include "rmw/rmw.h" +#include "rmw/sanity_checks.h" + +#include "rmw_dds_common/context.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_get_node_names( + const char * identifier, + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (RMW_RET_OK != rmw_check_zero_rmw_string_array(node_names)) { + return RMW_RET_INVALID_ARGUMENT; + } + if (RMW_RET_OK != rmw_check_zero_rmw_string_array(node_namespaces)) { + return RMW_RET_INVALID_ARGUMENT; + } + + auto common_context = static_cast(node->context->impl->common); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + return common_context->graph_cache.get_node_names( + node_names, + node_namespaces, + nullptr, + &allocator); +} + +rmw_ret_t +__rmw_get_node_names_with_enclaves( + const char * identifier, + const rmw_node_t * node, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * enclaves) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (RMW_RET_OK != rmw_check_zero_rmw_string_array(node_names)) { + return RMW_RET_INVALID_ARGUMENT; + } + if (RMW_RET_OK != rmw_check_zero_rmw_string_array(node_namespaces)) { + return RMW_RET_INVALID_ARGUMENT; + } + if (RMW_RET_OK != rmw_check_zero_rmw_string_array(enclaves)) { + return RMW_RET_INVALID_ARGUMENT; + } + + auto common_context = static_cast(node->context->impl->common); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + return common_context->graph_cache.get_node_names( + node_names, + node_namespaces, + enclaves, + &allocator); +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_publish.cpp b/src/rmw_publish.cpp new file mode 100644 index 0000000..c6033a2 --- /dev/null +++ b/src/rmw_publish.cpp @@ -0,0 +1,145 @@ +// 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/impl/cpp/macros.hpp" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +#include "tracetools/tracetools.h" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_publish( + const char * identifier, + const rmw_publisher_t * publisher, + const void * ros_message, + rmw_publisher_allocation_t * allocation) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); + + (void) allocation; + RMW_CHECK_FOR_NULL_WITH_MSG( + publisher, "publisher handle is null", + return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, publisher->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_FOR_NULL_WITH_MSG( + ros_message, "ros message handle is null", + return RMW_RET_INVALID_ARGUMENT); + + auto info = static_cast(publisher->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "publisher info pointer is null", return RMW_RET_ERROR); + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; + data.data = const_cast(ros_message); + data.impl = info->type_support_impl_; + TRACEPOINT(rmw_publish, ros_message); + if (!info->data_writer_->write(&data)) { + RMW_SET_ERROR_MSG("cannot publish data"); + return RMW_RET_ERROR; + } + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_publish_serialized_message( + const char * identifier, + const rmw_publisher_t * publisher, + const rmw_serialized_message_t * serialized_message, + rmw_publisher_allocation_t * allocation) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); + + (void) allocation; + RMW_CHECK_FOR_NULL_WITH_MSG( + publisher, "publisher handle is null", + return RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, publisher->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_FOR_NULL_WITH_MSG( + serialized_message, "serialized message handle is null", + return RMW_RET_INVALID_ARGUMENT); + + auto info = static_cast(publisher->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "publisher info pointer is null", return RMW_RET_ERROR); + + eprosima::fastcdr::FastBuffer buffer( + reinterpret_cast(serialized_message->buffer), serialized_message->buffer_length); + eprosima::fastcdr::Cdr ser( + buffer, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, eprosima::fastcdr::Cdr::DDS_CDR); + if (!ser.jump(serialized_message->buffer_length)) { + RMW_SET_ERROR_MSG("cannot correctly set serialized buffer"); + return RMW_RET_ERROR; + } + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = &ser; + data.impl = nullptr; // not used when is_cdr_buffer is true + if (!info->data_writer_->write(&data)) { + RMW_SET_ERROR_MSG("cannot publish data"); + return RMW_RET_ERROR; + } + + return RMW_RET_OK; +} +rmw_ret_t +__rmw_publish_loaned_message( + const char * identifier, + const rmw_publisher_t * publisher, + const void * ros_message, + rmw_publisher_allocation_t * allocation) +{ + static_cast(allocation); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); + + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, publisher->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!publisher->can_loan_messages) { + RMW_SET_ERROR_MSG("Loaning is not supported"); + return RMW_RET_UNSUPPORTED; + } + + RMW_CHECK_ARGUMENT_FOR_NULL(ros_message, RMW_RET_INVALID_ARGUMENT); + + auto info = static_cast(publisher->data); + if (!info->data_writer_->write(const_cast(ros_message))) { + RMW_SET_ERROR_MSG("cannot publish data"); + return RMW_RET_ERROR; + } + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_publisher.cpp b/src/rmw_publisher.cpp new file mode 100644 index 0000000..1fae29e --- /dev/null +++ b/src/rmw_publisher.cpp @@ -0,0 +1,217 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/rmw.h" + +#include "fastdds/dds/publisher/DataWriter.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_publisher_info.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/publisher.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +#include "time_utils.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_destroy_publisher( + const char * identifier, + const rmw_node_t * node, + rmw_publisher_t * publisher) +{ + assert(node->implementation_identifier == identifier); + assert(publisher->implementation_identifier == identifier); + + rmw_ret_t ret = RMW_RET_OK; + rmw_error_state_t error_state; + auto common_context = static_cast(node->context->impl->common); + auto info = static_cast(publisher->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_writer( + info->publisher_gid, common_context->gid, node->name, node->namespace_); + rmw_ret_t publish_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + &msg, + nullptr); + if (RMW_RET_OK != publish_ret) { + error_state = *rmw_get_error_state(); + ret = publish_ret; + rmw_reset_error(); + } + } + + auto participant_info = + static_cast(node->context->impl->participant_info); + rmw_ret_t inner_ret = destroy_publisher(identifier, participant_info, publisher); + 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; +} + +rmw_ret_t +__rmw_publisher_count_matched_subscriptions( + const rmw_publisher_t * publisher, + size_t * subscription_count) +{ + auto info = static_cast(publisher->data); + + *subscription_count = info->listener_->subscriptionCount(); + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_publisher_assert_liveliness( + const char * identifier, + const rmw_publisher_t * publisher) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, + publisher->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + auto info = static_cast(publisher->data); + if (nullptr == info) { + RMW_SET_ERROR_MSG("publisher internal data is invalid"); + return RMW_RET_ERROR; + } + + info->data_writer_->assert_liveliness(); + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_publisher_wait_for_all_acked( + const char * identifier, + const rmw_publisher_t * publisher, + rmw_time_t wait_timeout) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, + publisher->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + auto info = static_cast(publisher->data); + + eprosima::fastrtps::Duration_t timeout = rmw_time_to_fastrtps(wait_timeout); + + ReturnCode_t ret = info->data_writer_->wait_for_acknowledgments(timeout); + if (ReturnCode_t::RETCODE_OK == ret) { + return RMW_RET_OK; + } + + return RMW_RET_TIMEOUT; +} + +rmw_ret_t +__rmw_publisher_get_actual_qos( + const rmw_publisher_t * publisher, + rmw_qos_profile_t * qos) +{ + auto info = static_cast(publisher->data); + eprosima::fastdds::dds::DataWriter * fastdds_dw = info->data_writer_; + const eprosima::fastdds::dds::DataWriterQos & dds_qos = fastdds_dw->get_qos(); + + dds_qos_to_rmw_qos(dds_qos, qos); + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_borrow_loaned_message( + const char * identifier, + const rmw_publisher_t * publisher, + const rosidl_message_type_support_t * type_support, + void ** ros_message) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, publisher->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!publisher->can_loan_messages) { + RMW_SET_ERROR_MSG("Loaning is not supported"); + return RMW_RET_UNSUPPORTED; + } + + RMW_CHECK_ARGUMENT_FOR_NULL(type_support, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_message, RMW_RET_INVALID_ARGUMENT); + if (nullptr != *ros_message) { + return RMW_RET_INVALID_ARGUMENT; + } + + auto info = static_cast(publisher->data); + if (!info->data_writer_->loan_sample(*ros_message)) { + return RMW_RET_ERROR; + } + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_return_loaned_message_from_publisher( + const char * identifier, + const rmw_publisher_t * publisher, + void * loaned_message) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + publisher, publisher->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!publisher->can_loan_messages) { + RMW_SET_ERROR_MSG("Loaning is not supported"); + return RMW_RET_UNSUPPORTED; + } + + RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); + + auto info = static_cast(publisher->data); + if (!info->data_writer_->discard_loan(loaned_message)) { + return RMW_RET_ERROR; + } + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_qos.cpp b/src/rmw_qos.cpp new file mode 100644 index 0000000..3a635d0 --- /dev/null +++ b/src/rmw_qos.cpp @@ -0,0 +1,36 @@ +// 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/types.h" + +#include "rmw_dds_common/qos.hpp" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +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_dds_common::qos_profile_check_compatible( + publisher_profile, subscription_profile, compatibility, reason, reason_size); +} + +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_request.cpp b/src/rmw_request.cpp new file mode 100644 index 0000000..73ce6c5 --- /dev/null +++ b/src/rmw_request.cpp @@ -0,0 +1,121 @@ +// 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 "fastcdr/Cdr.h" +#include "fastcdr/FastBuffer.h" + +#include "fastdds/rtps/common/WriteParams.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_send_request( + const char * identifier, + const rmw_client_t * client, + const void * ros_request, + int64_t * sequence_id) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + client, + client->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(sequence_id, RMW_RET_INVALID_ARGUMENT); + + rmw_ret_t returnedValue = RMW_RET_ERROR; + + auto info = static_cast(client->data); + assert(info); + + eprosima::fastrtps::rtps::WriteParams wparams; + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; + data.data = const_cast(ros_request); + data.impl = info->request_type_support_impl_; + wparams.related_sample_identity().writer_guid() = info->reader_guid_; + if (info->request_writer_->write(&data, wparams)) { + returnedValue = RMW_RET_OK; + *sequence_id = ((int64_t)wparams.sample_identity().sequence_number().high) << 32 | + wparams.sample_identity().sequence_number().low; + } else { + RMW_SET_ERROR_MSG("cannot publish data"); + } + + return returnedValue; +} + +rmw_ret_t +__rmw_take_request( + const char * identifier, + const rmw_service_t * service, + rmw_service_info_t * request_header, + void * ros_request, + bool * taken) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + service, + service->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_request, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + + *taken = false; + + auto info = static_cast(service->data); + assert(info); + + CustomServiceRequest request = info->listener_->getRequest(); + + if (request.buffer_ != nullptr) { + auto raw_type_support = dynamic_cast( + info->response_type_support_.get()); + eprosima::fastcdr::Cdr deser(*request.buffer_, eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + if (raw_type_support->deserializeROSmessage( + deser, ros_request, info->request_type_support_impl_)) + { + // Get header + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + request.sample_identity_.writer_guid(), + request_header->request_id.writer_guid); + request_header->request_id.sequence_number = + ((int64_t)request.sample_identity_.sequence_number().high) << + 32 | request.sample_identity_.sequence_number().low; + request_header->source_timestamp = request.sample_info_.source_timestamp.to_ns(); + request_header->received_timestamp = request.sample_info_.source_timestamp.to_ns(); + *taken = true; + } + + delete request.buffer_; + } + + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_response.cpp b/src/rmw_response.cpp new file mode 100644 index 0000000..1766e4b --- /dev/null +++ b/src/rmw_response.cpp @@ -0,0 +1,146 @@ +// 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 "fastcdr/Cdr.h" + +#include "fastdds/rtps/common/WriteParams.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/impl/cpp/macros.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_take_response( + const char * identifier, + const rmw_client_t * client, + rmw_service_info_t * request_header, + void * ros_response, + bool * taken) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(client, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + client, + client->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + + *taken = false; + + auto info = static_cast(client->data); + assert(info); + + CustomClientResponse response; + + if (info->listener_->getResponse(response)) { + auto raw_type_support = dynamic_cast( + info->response_type_support_.get()); + eprosima::fastcdr::Cdr deser( + *response.buffer_, + eprosima::fastcdr::Cdr::DEFAULT_ENDIAN, + eprosima::fastcdr::Cdr::DDS_CDR); + if (raw_type_support->deserializeROSmessage( + deser, ros_response, info->response_type_support_impl_)) + { + request_header->source_timestamp = response.sample_info_.source_timestamp.to_ns(); + request_header->received_timestamp = response.sample_info_.reception_timestamp.to_ns(); + request_header->request_id.sequence_number = + ((int64_t)response.sample_identity_.sequence_number().high) << + 32 | response.sample_identity_.sequence_number().low; + + *taken = true; + } + } + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_send_response( + const char * identifier, + const rmw_service_t * service, + rmw_request_id_t * request_header, + void * ros_response) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(service, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + service, + service->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RMW_CHECK_ARGUMENT_FOR_NULL(request_header, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(ros_response, RMW_RET_INVALID_ARGUMENT); + + rmw_ret_t returnedValue = RMW_RET_ERROR; + + auto info = static_cast(service->data); + assert(info); + + eprosima::fastrtps::rtps::WriteParams wparams; + rmw_fastrtps_shared_cpp::copy_from_byte_array_to_fastrtps_guid( + request_header->writer_guid, + &wparams.related_sample_identity().writer_guid()); + wparams.related_sample_identity().sequence_number().high = + (int32_t)((request_header->sequence_number & 0xFFFFFFFF00000000) >> 32); + wparams.related_sample_identity().sequence_number().low = + (int32_t)(request_header->sequence_number & 0xFFFFFFFF); + + // TODO(MiguelCompany) The following block is a workaround for the race on the + // discovery of services. It is (ab)using a related_sample_identity on the request + // with the GUID of the response reader, so we can wait here for it to be matched to + // the server response writer. In the future, this should be done with the mechanism + // explained on OMG DDS-RPC 1.0 spec under section 7.6.2 (Enhanced Service Mapping) + + // According to the list of possible entity kinds in section 9.3.1.2 of RTPS + // readers will have this bit on, while writers will not. We use this to know + // if the related guid is the request writer or the response reader. + constexpr uint8_t entity_id_is_reader_bit = 0x04; + const eprosima::fastrtps::rtps::GUID_t & related_guid = + wparams.related_sample_identity().writer_guid(); + if ((related_guid.entityId.value[3] & entity_id_is_reader_bit) != 0) { + // Related guid is a reader, so it is the response subscription guid. + // Wait for the response writer to be matched with it. + auto listener = info->pub_listener_; + client_present_t ret = listener->check_for_subscription(related_guid); + if (ret == client_present_t::GONE) { + return RMW_RET_OK; + } else if (ret == client_present_t::MAYBE) { + RMW_SET_ERROR_MSG("client will not receive response"); + return RMW_RET_TIMEOUT; + } + } + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = false; + data.data = const_cast(ros_response); + data.impl = info->response_type_support_impl_; + if (info->response_writer_->write(&data, wparams)) { + returnedValue = RMW_RET_OK; + } else { + RMW_SET_ERROR_MSG("cannot publish data"); + } + + return returnedValue; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_security_logging.cpp b/src/rmw_security_logging.cpp new file mode 100644 index 0000000..de37aa8 --- /dev/null +++ b/src/rmw_security_logging.cpp @@ -0,0 +1,223 @@ +// Copyright 2020 Canonical Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rmw_fastrtps_shared_cpp/rmw_security_logging.hpp" + +#include +#include +#include +#include + +#include "fastdds/rtps/common/Property.h" +#include "fastdds/rtps/attributes/PropertyPolicy.h" + +#include "fastrtps/config.h" + +#include "rcutils/env.h" +#include "rcutils/filesystem.h" + +#include "rmw/error_handling.h" +#include "rmw/qos_profiles.h" +#include "rmw/types.h" + +#if HAVE_SECURITY + +namespace +{ +// Environment variable names +// TODO(security-wg): These are intended to be temporary, and need to be refactored into a proper +// abstraction. +const char log_file_variable_name[] = "ROS_SECURITY_LOG_FILE"; +const char log_publish_variable_name[] = "ROS_SECURITY_LOG_PUBLISH"; +const char log_verbosity_variable_name[] = "ROS_SECURITY_LOG_VERBOSITY"; + +// Logging properties +const char logging_plugin_property_name[] = "dds.sec.log.plugin"; +const char log_file_property_name[] = "dds.sec.log.builtin.DDS_LogTopic.log_file"; +const char verbosity_property_name[] = "dds.sec.log.builtin.DDS_LogTopic.logging_level"; +const char distribute_enable_property_name[] = + "dds.sec.log.builtin.DDS_LogTopic.distribute"; + +// Fast DDS supports the following verbosities: +// - EMERGENCY_LEVEL +// - ALERT_LEVEL +// - CRITICAL_LEVEL +// - ERROR_LEVEL +// - WARNING_LEVEL +// - NOTICE_LEVEL +// - INFORMATIONAL_LEVEL +// - DEBUG_LEVEL +// +// ROS has less logging levels, but it makes sense to use them here for consistency, so we have +// the following mapping. +const std::map verbosity_mapping { + {RCUTILS_LOG_SEVERITY_FATAL, "EMERGENCY_LEVEL"}, + {RCUTILS_LOG_SEVERITY_ERROR, "ERROR_LEVEL"}, + {RCUTILS_LOG_SEVERITY_WARN, "WARNING_LEVEL"}, + {RCUTILS_LOG_SEVERITY_INFO, "INFORMATIONAL_LEVEL"}, + {RCUTILS_LOG_SEVERITY_DEBUG, "DEBUG_LEVEL"}, +}; + +void severity_names_str(std::string & str) +{ + std::stringstream stream; + auto penultimate = --verbosity_mapping.crend(); + for (auto it = verbosity_mapping.crbegin(); it != penultimate; ++it) { + stream << g_rcutils_log_severity_names[it->first] << ", "; + } + + stream << "or " << g_rcutils_log_severity_names[penultimate->first]; + str = stream.str(); +} + +bool string_to_verbosity(const std::string & str, std::string & verbosity) +{ + int ros_severity; + if (rcutils_logging_severity_level_from_string( + str.c_str(), + rcutils_get_default_allocator(), &ros_severity) == RCUTILS_RET_OK) + { + try { + verbosity = verbosity_mapping.at(static_cast(ros_severity)); + return true; + } catch (std::out_of_range &) { + // Fall to the return below + } + } + + return false; +} + +bool validate_boolean(const std::string & str) +{ + return str == "true" || str == "false"; +} + +void add_property( + eprosima::fastrtps::rtps::PropertySeq & properties, + eprosima::fastrtps::rtps::Property && property) +{ + // Add property to vector. If property already exists, overwrite it. + std::string property_name = property.name(); + for (auto & existing_property : properties) { + if (existing_property.name() == property_name) { + existing_property = property; + return; + } + } + + properties.push_back(property); +} + +bool get_env(const std::string & variable_name, std::string & variable_value) +{ + const char * value; + const char * error_message = rcutils_get_env(variable_name.c_str(), &value); + if (error_message != NULL) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "unable to get %s environment variable: %s", + variable_name.c_str(), + error_message); + return false; + } + + variable_value = std::string(value); + + return true; +} +} // namespace + +#endif + +bool apply_security_logging_configuration(eprosima::fastrtps::rtps::PropertyPolicy & policy) +{ +#if HAVE_SECURITY + eprosima::fastrtps::rtps::PropertySeq properties; + std::string env_value; + + // Handle logging to file + if (!get_env(log_file_variable_name, env_value)) { + return false; + } + if (!env_value.empty()) { + add_property( + properties, + eprosima::fastrtps::rtps::Property( + log_file_property_name, env_value.c_str())); + } + + // Handle log distribution over DDS + if (!get_env(log_publish_variable_name, env_value)) { + return false; + } + if (!env_value.empty()) { + if (!validate_boolean(env_value)) { + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "%s is not valid: '%s' is not a supported value (use 'true' or 'false')", + log_publish_variable_name, + env_value.c_str()); + return false; + } + + add_property( + properties, + eprosima::fastrtps::rtps::Property( + distribute_enable_property_name, env_value.c_str())); + } + + // Handle log verbosity + if (!get_env(log_verbosity_variable_name, env_value)) { + return false; + } + if (!env_value.empty()) { + std::string verbosity; + if (!string_to_verbosity(env_value, verbosity)) { + std::string humanized_severity_list; + severity_names_str(humanized_severity_list); + + RMW_SET_ERROR_MSG_WITH_FORMAT_STRING( + "%s is not valid: %s is not a supported verbosity (use %s)", + log_verbosity_variable_name, + env_value.c_str(), + humanized_severity_list.c_str()); + return false; + } + + add_property( + properties, + eprosima::fastrtps::rtps::Property(verbosity_property_name, verbosity.c_str())); + } + + if (!properties.empty()) { + add_property( + properties, + eprosima::fastrtps::rtps::Property( + logging_plugin_property_name, + "builtin.DDS_LogTopic")); + } + + // Now that we're done parsing, actually update the properties + for (auto & item : properties) { + add_property(policy.properties(), std::move(item)); + } + + return true; +#else + (void)policy; + RMW_SET_ERROR_MSG( + "This Fast DDS version doesn't have the security libraries\n" + "Please compile Fast DDS using the -DSECURITY=ON CMake option"); + return false; +#endif +} diff --git a/src/rmw_service.cpp b/src/rmw_service.cpp new file mode 100644 index 0000000..92bd790 --- /dev/null +++ b/src/rmw_service.cpp @@ -0,0 +1,176 @@ +// 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 "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/create_rmw_gid.hpp" +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_destroy_service( + const char * identifier, + rmw_node_t * node, + rmw_service_t * service) +{ + rmw_ret_t final_ret = RMW_RET_OK; + + auto common_context = static_cast(node->context->impl->common); + auto participant_info = + static_cast(node->context->impl->participant_info); + auto info = static_cast(service->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_gid_t gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->request_reader_->guid()); + common_context->graph_cache.dissociate_reader( + gid, + common_context->gid, + node->name, + node->namespace_); + gid = rmw_fastrtps_shared_cpp::create_rmw_gid( + identifier, info->response_writer_->guid()); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_writer( + gid, common_context->gid, node->name, node->namespace_); + final_ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + } + + auto show_previous_error = + [&final_ret]() { + if (RMW_RET_OK != final_ret) { + RMW_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + rmw_reset_error(); + } + }; + + ///// + // Delete DataWriter and DataReader + { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Keep pointers to topics, so we can remove them later + auto response_topic = info->response_writer_->get_topic(); + auto request_topic = info->request_reader_->get_topicdescription(); + + // Delete DataReader + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->request_reader_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("Fail in delete datareader"); + final_ret = RMW_RET_ERROR; + info->request_reader_->set_listener(nullptr); + } + + // Delete DataReader listener + if (nullptr != info->listener_) { + delete info->listener_; + } + + // Delete DataWriter + ret = participant_info->publisher_->delete_datawriter(info->response_writer_); + if (ret != ReturnCode_t::RETCODE_OK) { + show_previous_error(); + RMW_SET_ERROR_MSG("Fail in delete datawriter"); + final_ret = RMW_RET_ERROR; + info->response_writer_->set_listener(nullptr); + } + + // Delete DataWriter listener + if (nullptr != info->pub_listener_) { + delete info->pub_listener_; + } + + // Delete topics and unregister types + remove_topic_and_type(participant_info, request_topic, info->request_type_support_); + remove_topic_and_type(participant_info, response_topic, info->response_type_support_); + + // Delete CustomServiceInfo structure + delete info; + } + + rmw_free(const_cast(service->service_name)); + rmw_service_free(service); + + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + return final_ret; +} + +rmw_ret_t +__rmw_service_response_publisher_get_actual_qos( + const rmw_service_t * service, + rmw_qos_profile_t * qos) +{ + auto srv = static_cast(service->data); + eprosima::fastdds::dds::DataWriter * fastdds_rw = srv->response_writer_; + dds_qos_to_rmw_qos(fastdds_rw->get_qos(), qos); + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_service_request_subscription_get_actual_qos( + const rmw_service_t * service, + rmw_qos_profile_t * qos) +{ + auto srv = static_cast(service->data); + eprosima::fastdds::dds::DataReader * fastdds_rr = srv->request_reader_; + dds_qos_to_rmw_qos(fastdds_rr->get_qos(), qos); + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_service_set_on_new_request_callback( + rmw_service_t * rmw_service, + rmw_event_callback_t callback, + const void * user_data) +{ + auto custom_service_info = static_cast(rmw_service->data); + custom_service_info->listener_->set_on_new_request_callback( + user_data, + callback); + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_service_names_and_types.cpp b/src/rmw_service_names_and_types.cpp new file mode 100644 index 0000000..f008ecd --- /dev/null +++ b/src/rmw_service_names_and_types.cpp @@ -0,0 +1,64 @@ +// 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 "rcutils/allocator.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/get_service_names_and_types.h" +#include "rmw/impl/cpp/macros.hpp" +#include "rmw/names_and_types.h" +#include "rmw/types.h" + +#include "rmw_dds_common/context.hpp" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +#include "demangle.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +rmw_ret_t +__rmw_get_service_names_and_types( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + rmw_names_and_types_t * service_names_and_types) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "allocator argument is invalid", return RMW_RET_INVALID_ARGUMENT); + if (RMW_RET_OK != rmw_names_and_types_check_zero(service_names_and_types)) { + return RMW_RET_INVALID_ARGUMENT; + } + + auto common_context = static_cast(node->context->impl->common); + + return common_context->graph_cache.get_names_and_types( + _demangle_service_from_topic, + _demangle_service_type_only, + allocator, + service_names_and_types); +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_service_server_is_available.cpp b/src/rmw_service_server_is_available.cpp new file mode 100644 index 0000000..4ef9784 --- /dev/null +++ b/src/rmw_service_server_is_available.cpp @@ -0,0 +1,130 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "fastrtps/subscriber/Subscriber.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/types.h" + +#include "rmw_dds_common/context.hpp" + +#include "demangle.hpp" +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_service_server_is_available( + const char * identifier, + const rmw_node_t * node, + const rmw_client_t * client, + bool * is_available) +{ + if (!node) { + RMW_SET_ERROR_MSG("node handle is null"); + return RMW_RET_ERROR; + } + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node handle, + node->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + if (!client) { + RMW_SET_ERROR_MSG("client handle is null"); + return RMW_RET_ERROR; + } + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + client handle, + client->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + if (!is_available) { + RMW_SET_ERROR_MSG("is_available is null"); + return RMW_RET_ERROR; + } + + auto client_info = static_cast(client->data); + if (!client_info) { + RMW_SET_ERROR_MSG("client info handle is null"); + return RMW_RET_ERROR; + } + + auto pub_topic_name = client_info->request_topic_; + + auto sub_topic_name = client_info->response_topic_; + + *is_available = false; + auto common_context = static_cast(node->context->impl->common); + + size_t number_of_request_subscribers = 0; + rmw_ret_t ret = + common_context->graph_cache.get_reader_count(pub_topic_name, &number_of_request_subscribers); + if (ret != RMW_RET_OK) { + // error + return ret; + } + if (0 == number_of_request_subscribers) { + // not ready + return RMW_RET_OK; + } + + size_t number_of_response_publishers = 0; + ret = + common_context->graph_cache.get_writer_count(sub_topic_name, &number_of_response_publishers); + if (ret != RMW_RET_OK) { + // error + return ret; + } + if (0 == number_of_response_publishers) { + // not ready + return RMW_RET_OK; + } + + if (number_of_request_subscribers != number_of_response_publishers) { + // not ready + return RMW_RET_OK; + } + + size_t matched_request_pubs = client_info->request_publisher_matched_count_.load(); + if (0 == matched_request_pubs) { + // not ready + return RMW_RET_OK; + } + size_t matched_response_subs = client_info->response_subscriber_matched_count_.load(); + if (0 == matched_response_subs) { + // not ready + return RMW_RET_OK; + } + if (matched_request_pubs != matched_response_subs) { + // not ready + return RMW_RET_OK; + } + + // all conditions met, there is a service server available + *is_available = true; + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_subscription.cpp b/src/rmw_subscription.cpp new file mode 100644 index 0000000..a521b1a --- /dev/null +++ b/src/rmw_subscription.cpp @@ -0,0 +1,275 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// Copyright 2016-2018 Proyectos y Sistemas de Mantenimiento SL (eProsima). +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rcpputils/scope_exit.hpp" + +#include "fastdds/dds/subscriber/DataReader.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_destroy_subscription( + const char * identifier, + const rmw_node_t * node, + rmw_subscription_t * subscription, + bool reset_cft) +{ + assert(node->implementation_identifier == identifier); + assert(subscription->implementation_identifier == identifier); + + rmw_ret_t ret = RMW_RET_OK; + rmw_error_state_t error_state; + rmw_error_string_t error_string; + auto common_context = static_cast(node->context->impl->common); + auto info = static_cast(subscription->data); + { + // Update graph + std::lock_guard guard(common_context->node_update_mutex); + rmw_dds_common::msg::ParticipantEntitiesInfo msg = + common_context->graph_cache.dissociate_reader( + info->subscription_gid_, common_context->gid, node->name, node->namespace_); + ret = rmw_fastrtps_shared_cpp::__rmw_publish( + identifier, + common_context->pub, + static_cast(&msg), + nullptr); + if (RMW_RET_OK != ret) { + error_state = *rmw_get_error_state(); + error_string = rmw_get_error_string(); + rmw_reset_error(); + } + } + + auto participant_info = + static_cast(node->context->impl->participant_info); + rmw_ret_t local_ret = destroy_subscription(identifier, participant_info, subscription, reset_cft); + if (RMW_RET_OK != local_ret) { + if (RMW_RET_OK != ret) { + RMW_SAFE_FWRITE_TO_STDERR(error_string.str); + RMW_SAFE_FWRITE_TO_STDERR(" during '" RCUTILS_STRINGIFY(__function__) "'\n"); + } + ret = local_ret; + } else if (RMW_RET_OK != ret) { + rmw_set_error_state(error_state.message, error_state.file, error_state.line_number); + } + return ret; +} + +rmw_ret_t +__rmw_subscription_count_matched_publishers( + const rmw_subscription_t * subscription, + size_t * publisher_count) +{ + auto info = static_cast(subscription->data); + + *publisher_count = info->listener_->publisherCount(); + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_subscription_get_actual_qos( + const rmw_subscription_t * subscription, + rmw_qos_profile_t * qos) +{ + auto info = static_cast(subscription->data); + eprosima::fastdds::dds::DataReader * fastdds_dr = info->data_reader_; + const eprosima::fastdds::dds::DataReaderQos & dds_qos = fastdds_dr->get_qos(); + + dds_qos_to_rmw_qos(dds_qos, qos); + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_subscription_set_content_filter( + rmw_subscription_t * subscription, + const rmw_subscription_content_filter_options_t * options +) +{ + auto info = static_cast(subscription->data); + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = info->filtered_topic_; + const bool filter_expression_empty = (*options->filter_expression == '\0'); + + if (!filtered_topic && filter_expression_empty) { + // can't reset current subscriber + RMW_SET_ERROR_MSG( + "current subscriber has no content filter topic"); + return RMW_RET_ERROR; + } else if (filtered_topic && !filter_expression_empty) { + std::vector expression_parameters; + for (size_t i = 0; i < options->expression_parameters.size; ++i) { + expression_parameters.push_back(options->expression_parameters.data[i]); + } + + ReturnCode_t ret = + filtered_topic->set_filter_expression(options->filter_expression, expression_parameters); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG( + "failed to set_filter_expression"); + return RMW_RET_ERROR; + } + return RMW_RET_OK; + } + + eprosima::fastdds::dds::DomainParticipant * dds_participant = + info->dds_participant_; + eprosima::fastdds::dds::TopicDescription * des_topic = nullptr; + const char * eprosima_fastrtps_identifier = subscription->implementation_identifier; + + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_destroy_subscription( + eprosima_fastrtps_identifier, + info->node_, + subscription, + true /* reset_cft */); + if (ret != RMW_RET_OK) { + RMW_SET_ERROR_MSG("delete subscription with reset cft"); + return RMW_RET_ERROR; + } + + if (!filtered_topic) { + // create content filtered topic + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = nullptr; + if (!rmw_fastrtps_shared_cpp::create_content_filtered_topic( + dds_participant, info->topic_, + info->topic_name_mangled_, options, &filtered_topic)) + { + RMW_SET_ERROR_MSG("create_contentfilteredtopic() failed to create contentfilteredtopic"); + return RMW_RET_ERROR; + } + info->filtered_topic_ = filtered_topic; + des_topic = filtered_topic; + } else { + // use the existing parent topic + des_topic = info->topic_; + } + + // create data reader + eprosima::fastdds::dds::Subscriber * subscriber = info->subscriber_; + const rmw_subscription_options_t * subscription_options = + &subscription->options; + if (!rmw_fastrtps_shared_cpp::create_datareader( + info->datareader_qos_, + subscription_options, + subscriber, + des_topic, + info->listener_, + &info->data_reader_)) + { + RMW_SET_ERROR_MSG("create_datareader() could not create data reader"); + return RMW_RET_ERROR; + } + // lambda to delete datareader + auto cleanup_datareader = rcpputils::make_scope_exit( + [subscriber, info]() { + subscriber->delete_datareader(info->data_reader_); + }); + + ///// + // Update RMW GID + info->subscription_gid_ = rmw_fastrtps_shared_cpp::create_rmw_gid( + eprosima_fastrtps_identifier, info->data_reader_->guid()); + + { + rmw_dds_common::Context * common_context = info->common_context_; + const rmw_node_t * node = info->node_; + + // 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) { + static_cast(common_context->graph_cache.dissociate_writer( + info->subscription_gid_, common_context->gid, node->name, node->namespace_)); + return RMW_RET_ERROR; + } + } + cleanup_datareader.cancel(); + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_subscription_get_content_filter( + const rmw_subscription_t * subscription, + rcutils_allocator_t * allocator, + rmw_subscription_content_filter_options_t * options +) +{ + auto info = static_cast(subscription->data); + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = info->filtered_topic_; + + if (nullptr == filtered_topic) { + RMW_SET_ERROR_MSG("this subscriber has not created a ContentFilteredTopic"); + return RMW_RET_ERROR; + } + std::vector expression_parameters; + ReturnCode_t ret = filtered_topic->get_expression_parameters(expression_parameters); + if (ret != ReturnCode_t::RETCODE_OK) { + RMW_SET_ERROR_MSG( + "failed to get_expression_parameters"); + return RMW_RET_ERROR; + } + + std::vector string_array; + for (size_t i = 0; i < expression_parameters.size(); ++i) { + string_array.push_back(expression_parameters[i].c_str()); + } + + return rmw_subscription_content_filter_options_init( + filtered_topic->get_filter_expression().c_str(), + string_array.size(), + string_array.data(), + allocator, + options + ); +} + +rmw_ret_t +__rmw_subscription_set_on_new_message_callback( + rmw_subscription_t * rmw_subscription, + rmw_event_callback_t callback, + const void * user_data) +{ + auto custom_subscriber_info = static_cast(rmw_subscription->data); + custom_subscriber_info->listener_->set_on_new_message_callback( + user_data, + callback); + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_take.cpp b/src/rmw_take.cpp new file mode 100644 index 0000000..e4e3dfb --- /dev/null +++ b/src/rmw_take.cpp @@ -0,0 +1,539 @@ +// 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/serialized_message.h" +#include "rmw/rmw.h" + +#include "fastdds/dds/subscriber/SampleInfo.hpp" + +#include "fastrtps/utils/collections/ResourceLimitedVector.hpp" + +#include "fastcdr/Cdr.h" +#include "fastcdr/FastBuffer.h" + +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" + +#include "tracetools/tracetools.h" + +namespace rmw_fastrtps_shared_cpp +{ + +using DataSharingKind = eprosima::fastdds::dds::DataSharingKind; + +void +_assign_message_info( + const char * identifier, + rmw_message_info_t * message_info, + const eprosima::fastdds::dds::SampleInfo * sinfo) +{ + message_info->source_timestamp = sinfo->source_timestamp.to_ns(); + message_info->received_timestamp = sinfo->reception_timestamp.to_ns(); + auto fastdds_sn = sinfo->sample_identity.sequence_number(); + message_info->publication_sequence_number = + (static_cast(fastdds_sn.high) << 32) | + static_cast(fastdds_sn.low); + message_info->reception_sequence_number = RMW_MESSAGE_INFO_SEQUENCE_NUMBER_UNSUPPORTED; + rmw_gid_t * sender_gid = &message_info->publisher_gid; + sender_gid->implementation_identifier = identifier; + memset(sender_gid->data, 0, RMW_GID_STORAGE_SIZE); + + rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array( + sinfo->sample_identity.writer_guid(), + sender_gid->data); +} + +rmw_ret_t +_take( + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + (void) allocation; + *taken = false; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) + + auto info = static_cast(subscription->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); + + eprosima::fastdds::dds::SampleInfo sinfo; + + rmw_fastrtps_shared_cpp::SerializedData data; + + data.is_cdr_buffer = false; + data.data = ros_message; + data.impl = info->type_support_impl_; + + while (0 < info->data_reader_->get_unread_count()) { + if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { + // Update hasData from listener + info->listener_->update_has_data(info->data_reader_); + + if (subscription->options.ignore_local_publications) { + auto sample_writer_guid = + eprosima::fastrtps::rtps::iHandle2GUID(sinfo.publication_handle); + + if (sample_writer_guid.guidPrefix == info->data_reader_->guid().guidPrefix) { + // This is a local publication. Ignore it + continue; + } + } + + if (sinfo.valid_data) { + if (message_info) { + _assign_message_info(identifier, message_info, &sinfo); + } + *taken = true; + break; + } + } + } + + TRACEPOINT( + rmw_take, + static_cast(subscription), + static_cast(ros_message), + (message_info ? message_info->source_timestamp : 0LL), + *taken); + return RMW_RET_OK; +} + +rmw_ret_t +_take_sequence( + const char * identifier, + 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) +{ + *taken = 0; + bool taken_flag = false; + rmw_ret_t ret = RMW_RET_OK; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + auto info = static_cast(subscription->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); + + // Limit the upper bound of reads to the number unread at the beginning. + // This prevents any samples that are added after the beginning of the + // _take_sequence call from being read. + auto unread_count = info->data_reader_->get_unread_count(); + if (unread_count < count) { + count = unread_count; + } + + for (size_t ii = 0; ii < count; ++ii) { + taken_flag = false; + ret = _take( + identifier, subscription, message_sequence->data[*taken], + &taken_flag, &message_info_sequence->data[*taken], allocation); + + if (ret != RMW_RET_OK) { + break; + } + + if (taken_flag) { + (*taken)++; + } + } + + message_sequence->size = *taken; + message_info_sequence->size = *taken; + + return ret; +} + +rmw_ret_t +__rmw_take_event( + const char * identifier, + const rmw_event_t * event_handle, + void * event_info, + bool * taken) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(event_handle, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(event_info, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + + *taken = false; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + event handle, + event_handle->implementation_identifier, + identifier, + return RMW_RET_ERROR); + + auto event = static_cast(event_handle->data); + if (event->getListener()->takeNextEvent(event_handle->event_type, event_info)) { + *taken = true; + return RMW_RET_OK; + } + + return RMW_RET_ERROR; +} + +rmw_ret_t +__rmw_take( + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_subscription_allocation_t * allocation) +{ + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_ARGUMENT_FOR_NULL( + ros_message, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); + + return _take(identifier, subscription, ros_message, taken, nullptr, allocation); +} + +rmw_ret_t +__rmw_take_sequence( + const char * identifier, + const rmw_subscription_t * subscription, + size_t count, + rmw_message_sequence_t * message_sequence, + rmw_message_info_sequence_t * message_info_sequence, + size_t * taken, + rmw_subscription_allocation_t * allocation) +{ + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_ARGUMENT_FOR_NULL( + message_sequence, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_ARGUMENT_FOR_NULL( + message_info_sequence, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); + + if (0u == count) { + RMW_SET_ERROR_MSG("count cannot be 0"); + return RMW_RET_INVALID_ARGUMENT; + } + + if (count > message_sequence->capacity) { + RMW_SET_ERROR_MSG("Insufficient capacity in message_sequence"); + return RMW_RET_INVALID_ARGUMENT; + } + + if (count > message_info_sequence->capacity) { + RMW_SET_ERROR_MSG("Insufficient capacity in message_info_sequence"); + return RMW_RET_INVALID_ARGUMENT; + } + + return _take_sequence( + identifier, subscription, count, message_sequence, message_info_sequence, + taken, allocation); +} + +rmw_ret_t +__rmw_take_with_info( + const char * identifier, + const rmw_subscription_t * subscription, + void * ros_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + RMW_CHECK_ARGUMENT_FOR_NULL( + message_info, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_ARGUMENT_FOR_NULL( + ros_message, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); + + return _take(identifier, subscription, ros_message, taken, message_info, allocation); +} + +rmw_ret_t +_take_serialized_message( + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + (void) allocation; + *taken = false; + + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription handle, + subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) + + auto info = static_cast(subscription->data); + RCUTILS_CHECK_FOR_NULL_WITH_MSG(info, "custom subscriber info is null", return RMW_RET_ERROR); + + eprosima::fastcdr::FastBuffer buffer; + eprosima::fastdds::dds::SampleInfo sinfo; + + rmw_fastrtps_shared_cpp::SerializedData data; + data.is_cdr_buffer = true; + data.data = &buffer; + data.impl = nullptr; // not used when is_cdr_buffer is true + + if (info->data_reader_->take_next_sample(&data, &sinfo) == ReturnCode_t::RETCODE_OK) { + // Update hasData from listener + info->listener_->update_has_data(info->data_reader_); + + if (sinfo.valid_data) { + auto buffer_size = static_cast(buffer.getBufferSize()); + if (serialized_message->buffer_capacity < buffer_size) { + auto ret = rmw_serialized_message_resize(serialized_message, buffer_size); + if (ret != RMW_RET_OK) { + return ret; // Error message already set + } + } + serialized_message->buffer_length = buffer_size; + memcpy(serialized_message->buffer, buffer.getBuffer(), serialized_message->buffer_length); + + if (message_info) { + _assign_message_info(identifier, message_info, &sinfo); + } + *taken = true; + } + } + + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_take_serialized_message( + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_subscription_allocation_t * allocation) +{ + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_ARGUMENT_FOR_NULL( + serialized_message, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); + + return _take_serialized_message( + identifier, subscription, serialized_message, taken, nullptr, allocation); +} + +rmw_ret_t +__rmw_take_serialized_message_with_info( + const char * identifier, + const rmw_subscription_t * subscription, + rmw_serialized_message_t * serialized_message, + bool * taken, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + RMW_CHECK_ARGUMENT_FOR_NULL( + subscription, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_ARGUMENT_FOR_NULL( + serialized_message, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_ARGUMENT_FOR_NULL( + taken, RMW_RET_INVALID_ARGUMENT); + + RMW_CHECK_ARGUMENT_FOR_NULL( + message_info, RMW_RET_INVALID_ARGUMENT); + + return _take_serialized_message( + identifier, subscription, serialized_message, taken, message_info, allocation); +} + +// ----------------- Loans related code ------------------------- // + +struct GenericSequence : public eprosima::fastdds::dds::LoanableCollection +{ + GenericSequence() = default; + + void resize(size_type /*new_length*/) override + { + // This kind of collection should only be used with loans + throw std::bad_alloc(); + } +}; + +struct LoanManager +{ + struct Item + { + GenericSequence data_seq{}; + eprosima::fastdds::dds::SampleInfoSeq info_seq{}; + }; + + explicit LoanManager(const eprosima::fastrtps::ResourceLimitedContainerConfig & items_cfg) + : items(items_cfg) + { + } + + void add_item(std::unique_ptr item) + { + std::lock_guard guard(mtx); + items.push_back(std::move(item)); + } + + std::unique_ptr erase_item(void * loaned_message) + { + std::unique_ptr ret{nullptr}; + + std::lock_guard guard(mtx); + for (auto it = items.begin(); it != items.end(); ++it) { + if (loaned_message == (*it)->data_seq.buffer()[0]) { + ret = std::move(*it); + items.erase(it); + break; + } + } + + return ret; + } + +private: + std::mutex mtx; + using ItemVector = eprosima::fastrtps::ResourceLimitedVector>; + ItemVector items RCPPUTILS_TSA_GUARDED_BY(mtx); +}; + +void +__init_subscription_for_loans( + rmw_subscription_t * subscription) +{ + auto info = static_cast(subscription->data); + const auto & qos = info->data_reader_->get_qos(); + bool has_data_sharing = DataSharingKind::OFF != qos.data_sharing().kind(); + subscription->can_loan_messages = has_data_sharing && info->type_support_->is_plain(); + if (subscription->can_loan_messages) { + const auto & allocation_qos = qos.reader_resource_limits().outstanding_reads_allocation; + info->loan_manager_ = std::make_shared(allocation_qos); + } +} + +rmw_ret_t +__rmw_take_loaned_message_internal( + const char * identifier, + const rmw_subscription_t * subscription, + void ** loaned_message, + bool * taken, + rmw_message_info_t * message_info) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!subscription->can_loan_messages) { + RMW_SET_ERROR_MSG("Loaning is not supported"); + return RMW_RET_UNSUPPORTED; + } + + RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_ARGUMENT_FOR_NULL(taken, RMW_RET_INVALID_ARGUMENT); + + auto info = static_cast(subscription->data); + + auto item = std::make_unique(); + + while (ReturnCode_t::RETCODE_OK == info->data_reader_->take(item->data_seq, item->info_seq, 1)) { + if (item->info_seq[0].valid_data) { + if (nullptr != message_info) { + _assign_message_info(identifier, message_info, &item->info_seq[0]); + } + *loaned_message = item->data_seq.buffer()[0]; + *taken = true; + info->listener_->update_has_data(info->data_reader_); + + info->loan_manager_->add_item(std::move(item)); + + return RMW_RET_OK; + } + + // Should return loan before taking again + info->data_reader_->return_loan(item->data_seq, item->info_seq); + } + + // No data available, return loan information. + *taken = false; + info->listener_->update_has_data(info->data_reader_); + return RMW_RET_OK; +} + +rmw_ret_t +__rmw_return_loaned_message_from_subscription( + const char * identifier, + const rmw_subscription_t * subscription, + void * loaned_message) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + subscription, subscription->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + if (!subscription->can_loan_messages) { + RMW_SET_ERROR_MSG("Loaning is not supported"); + return RMW_RET_UNSUPPORTED; + } + RMW_CHECK_ARGUMENT_FOR_NULL(loaned_message, RMW_RET_INVALID_ARGUMENT); + + auto info = static_cast(subscription->data); + std::unique_ptr item; + item = info->loan_manager_->erase_item(loaned_message); + if (item != nullptr) { + if (!info->data_reader_->return_loan(item->data_seq, item->info_seq)) { + RMW_SET_ERROR_MSG("Error returning loan"); + return RMW_RET_ERROR; + } + + return RMW_RET_OK; + } + + RMW_SET_ERROR_MSG("Trying to return message not loaned by this subscription"); + return RMW_RET_ERROR; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_topic_names_and_types.cpp b/src/rmw_topic_names_and_types.cpp new file mode 100644 index 0000000..5ac6dc4 --- /dev/null +++ b/src/rmw_topic_names_and_types.cpp @@ -0,0 +1,72 @@ +// 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 "rcutils/allocator.h" + +#include "rmw/allocators.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/types.h" + +#include "rmw_dds_common/context.hpp" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_context_impl.hpp" + +#include "demangle.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +rmw_ret_t +__rmw_get_topic_names_and_types( + const char * identifier, + const rmw_node_t * node, + rcutils_allocator_t * allocator, + bool no_demangle, + rmw_names_and_types_t * topic_names_and_types) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(node, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + node, + node->implementation_identifier, + identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + allocator, "allocator argument is invalid", return RMW_RET_INVALID_ARGUMENT); + if (RMW_RET_OK != rmw_names_and_types_check_zero(topic_names_and_types)) { + return RMW_RET_INVALID_ARGUMENT; + } + + DemangleFunction demangle_topic = _demangle_ros_topic_from_topic; + DemangleFunction demangle_type = _demangle_if_ros_type; + + if (no_demangle) { + demangle_topic = _identity_demangle; + demangle_type = _identity_demangle; + } + auto common_context = static_cast(node->context->impl->common); + + return common_context->graph_cache.get_names_and_types( + demangle_topic, + demangle_type, + allocator, + topic_names_and_types); +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_trigger_guard_condition.cpp b/src/rmw_trigger_guard_condition.cpp new file mode 100644 index 0000000..ac3c720 --- /dev/null +++ b/src/rmw_trigger_guard_condition.cpp @@ -0,0 +1,41 @@ +// 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/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "types/guard_condition.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_trigger_guard_condition( + const char * identifier, + const rmw_guard_condition_t * guard_condition_handle) +{ + assert(guard_condition_handle); + + if (guard_condition_handle->implementation_identifier != identifier) { + RMW_SET_ERROR_MSG("guard condition handle not from this implementation"); + return RMW_RET_ERROR; + } + + auto guard_condition = static_cast(guard_condition_handle->data); + guard_condition->trigger(); + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_wait.cpp b/src/rmw_wait.cpp new file mode 100644 index 0000000..4275e4f --- /dev/null +++ b/src/rmw_wait.cpp @@ -0,0 +1,251 @@ +// 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 "rcutils/macros.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/custom_client_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_service_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "types/custom_wait_set_info.hpp" +#include "types/guard_condition.hpp" + +// helper function for wait +bool +check_wait_set_for_data( + const rmw_subscriptions_t * subscriptions, + const rmw_guard_conditions_t * guard_conditions, + const rmw_services_t * services, + const rmw_clients_t * clients, + const rmw_events_t * events) +{ + if (subscriptions) { + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + void * data = subscriptions->subscribers[i]; + auto custom_subscriber_info = static_cast(data); + // Short circuiting out of this function is possible + if (custom_subscriber_info && custom_subscriber_info->listener_->hasData()) { + return true; + } + } + } + + if (clients) { + for (size_t i = 0; i < clients->client_count; ++i) { + void * data = clients->clients[i]; + auto custom_client_info = static_cast(data); + if (custom_client_info && custom_client_info->listener_->hasData()) { + return true; + } + } + } + + if (services) { + for (size_t i = 0; i < services->service_count; ++i) { + void * data = services->services[i]; + auto custom_service_info = static_cast(data); + if (custom_service_info && custom_service_info->listener_->hasData()) { + return true; + } + } + } + + if (events) { + for (size_t i = 0; i < events->event_count; ++i) { + auto event = static_cast(events->events[i]); + auto custom_event_info = static_cast(event->data); + if (custom_event_info->getListener()->hasEvent(event->event_type)) { + return true; + } + } + } + + if (guard_conditions) { + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + void * data = guard_conditions->guard_conditions[i]; + auto guard_condition = static_cast(data); + if (guard_condition && guard_condition->hasTriggered()) { + return true; + } + } + } + return false; +} + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +__rmw_wait( + const char * identifier, + 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) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INCORRECT_RMW_IMPLEMENTATION); + + RMW_CHECK_ARGUMENT_FOR_NULL(wait_set, RMW_RET_INVALID_ARGUMENT); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + wait set handle, + wait_set->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) + + // If wait_set_info is ever nullptr, it can only mean one of three things: + // - Wait set is invalid. Caller did not respect preconditions. + // - Implementation is logically broken. Definitely not something we want to treat as a normal + // error. + // - Heap is corrupt. + // In all three cases, it's better if this crashes soon enough. + auto wait_set_info = static_cast(wait_set->data); + std::mutex * conditionMutex = &wait_set_info->condition_mutex; + std::condition_variable * conditionVariable = &wait_set_info->condition; + + if (subscriptions) { + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + void * data = subscriptions->subscribers[i]; + auto custom_subscriber_info = static_cast(data); + custom_subscriber_info->listener_->attachCondition(conditionMutex, conditionVariable); + } + } + + if (clients) { + for (size_t i = 0; i < clients->client_count; ++i) { + void * data = clients->clients[i]; + auto custom_client_info = static_cast(data); + custom_client_info->listener_->attachCondition(conditionMutex, conditionVariable); + } + } + + if (services) { + for (size_t i = 0; i < services->service_count; ++i) { + void * data = services->services[i]; + auto custom_service_info = static_cast(data); + custom_service_info->listener_->attachCondition(conditionMutex, conditionVariable); + } + } + + if (events) { + for (size_t i = 0; i < events->event_count; ++i) { + auto event = static_cast(events->events[i]); + auto custom_event_info = static_cast(event->data); + custom_event_info->getListener()->attachCondition(conditionMutex, conditionVariable); + } + } + + if (guard_conditions) { + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + void * data = guard_conditions->guard_conditions[i]; + auto guard_condition = static_cast(data); + guard_condition->attachCondition(conditionMutex, conditionVariable); + } + } + + // This mutex prevents any of the listeners + // to change the internal state and notify the condition + // between the call to hasData() / hasTriggered() and wait() + // otherwise the decision to wait might be incorrect + std::unique_lock lock(*conditionMutex); + + bool hasData = check_wait_set_for_data( + subscriptions, guard_conditions, services, clients, events); + auto predicate = [subscriptions, guard_conditions, services, clients, events]() { + return check_wait_set_for_data(subscriptions, guard_conditions, services, clients, events); + }; + + bool timeout = false; + if (!hasData) { + if (!wait_timeout) { + conditionVariable->wait(lock, predicate); + } else if (wait_timeout->sec > 0 || wait_timeout->nsec > 0) { + auto n = std::chrono::duration_cast( + std::chrono::seconds(wait_timeout->sec)); + n += std::chrono::nanoseconds(wait_timeout->nsec); + timeout = !conditionVariable->wait_for(lock, n, predicate); + } else { + timeout = true; + } + } + + // Unlock the condition variable mutex to prevent deadlocks that can occur if + // a listener triggers while the condition variable is being detached. + // Listeners will no longer be prevented from changing their internal state, + // but that should not cause issues (if a listener has data / has triggered + // after we check, it will be caught on the next call to this function). + lock.unlock(); + + if (subscriptions) { + for (size_t i = 0; i < subscriptions->subscriber_count; ++i) { + void * data = subscriptions->subscribers[i]; + auto custom_subscriber_info = static_cast(data); + custom_subscriber_info->listener_->detachCondition(); + if (!custom_subscriber_info->listener_->hasData()) { + subscriptions->subscribers[i] = 0; + } + } + } + + if (clients) { + for (size_t i = 0; i < clients->client_count; ++i) { + void * data = clients->clients[i]; + auto custom_client_info = static_cast(data); + custom_client_info->listener_->detachCondition(); + if (!custom_client_info->listener_->hasData()) { + clients->clients[i] = 0; + } + } + } + + if (services) { + for (size_t i = 0; i < services->service_count; ++i) { + void * data = services->services[i]; + auto custom_service_info = static_cast(data); + custom_service_info->listener_->detachCondition(); + if (!custom_service_info->listener_->hasData()) { + services->services[i] = 0; + } + } + } + + if (events) { + for (size_t i = 0; i < events->event_count; ++i) { + auto event = static_cast(events->events[i]); + auto custom_event_info = static_cast(event->data); + custom_event_info->getListener()->detachCondition(); + if (!custom_event_info->getListener()->hasEvent(event->event_type)) { + events->events[i] = nullptr; + } + } + } + + if (guard_conditions) { + for (size_t i = 0; i < guard_conditions->guard_condition_count; ++i) { + void * data = guard_conditions->guard_conditions[i]; + auto guard_condition = static_cast(data); + guard_condition->detachCondition(); + if (!guard_condition->getHasTriggered()) { + guard_conditions->guard_conditions[i] = 0; + } + } + } + + return timeout ? RMW_RET_TIMEOUT : RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/rmw_wait_set.cpp b/src/rmw_wait_set.cpp new file mode 100644 index 0000000..c29137f --- /dev/null +++ b/src/rmw_wait_set.cpp @@ -0,0 +1,106 @@ +// 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 "rcutils/macros.h" + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/impl/cpp/macros.hpp" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +#include "types/custom_wait_set_info.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_wait_set_t * +__rmw_create_wait_set(const char * identifier, rmw_context_t * context, size_t max_conditions) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(nullptr); + RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, NULL); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + init context, + context->implementation_identifier, + identifier, + // TODO(wjwwood): replace this with RMW_RET_INCORRECT_RMW_IMPLEMENTATION when refactored + return nullptr); + + (void)max_conditions; + + // From here onward, error results in unrolling in the goto fail block. + CustomWaitsetInfo * wait_set_info = nullptr; + rmw_wait_set_t * wait_set = rmw_wait_set_allocate(); + if (!wait_set) { + RMW_SET_ERROR_MSG("failed to allocate wait set"); + goto fail; + } + wait_set->implementation_identifier = identifier; + wait_set->data = rmw_allocate(sizeof(CustomWaitsetInfo)); + if (!wait_set->data) { + RMW_SET_ERROR_MSG("failed to allocate wait set info"); + goto fail; + } + // This should default-construct the fields of CustomWaitsetInfo + RMW_TRY_PLACEMENT_NEW( + wait_set_info, + wait_set->data, + goto fail, + // cppcheck-suppress syntaxError + CustomWaitsetInfo, ); + (void) wait_set_info; + + return wait_set; + +fail: + if (wait_set) { + if (wait_set->data) { + rmw_free(wait_set->data); + } + rmw_wait_set_free(wait_set); + } + return nullptr; +} + +rmw_ret_t +__rmw_destroy_wait_set(const char * identifier, rmw_wait_set_t * wait_set) +{ + RMW_CHECK_ARGUMENT_FOR_NULL(wait_set, RMW_RET_ERROR); + RMW_CHECK_TYPE_IDENTIFIERS_MATCH( + wait set handle, + wait_set->implementation_identifier, identifier, + return RMW_RET_INCORRECT_RMW_IMPLEMENTATION) + + auto result = RMW_RET_OK; + // If wait_set_info is ever nullptr, it can only mean one of three things: + // - Wait set is invalid. Caller did not respect preconditions. + // - Implementation is logically broken. Definitely not something we want to treat as a normal + // error. + // - Heap is corrupt. + // In all three cases, it's better if this crashes soon enough. + auto wait_set_info = static_cast(wait_set->data); + + if (wait_set->data) { + if (wait_set_info) { + RMW_TRY_DESTRUCTOR( + wait_set_info->~CustomWaitsetInfo(), wait_set_info, result = RMW_RET_ERROR) + } + rmw_free(wait_set->data); + } + rmw_wait_set_free(wait_set); + + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + return result; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/subscription.cpp b/src/subscription.cpp new file mode 100644 index 0000000..bfb6254 --- /dev/null +++ b/src/subscription.cpp @@ -0,0 +1,84 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rmw/allocators.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "rmw_fastrtps_shared_cpp/custom_participant_info.hpp" +#include "rmw_fastrtps_shared_cpp/custom_subscriber_info.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" +#include "rmw_fastrtps_shared_cpp/qos.hpp" +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" +#include "rmw_fastrtps_shared_cpp/subscription.hpp" +#include "rmw_fastrtps_shared_cpp/TypeSupport.hpp" +#include "rmw_fastrtps_shared_cpp/utils.hpp" + +namespace rmw_fastrtps_shared_cpp +{ +rmw_ret_t +destroy_subscription( + const char * identifier, + CustomParticipantInfo * participant_info, + rmw_subscription_t * subscription, + bool reset_cft) +{ + assert(subscription->implementation_identifier == identifier); + static_cast(identifier); + + { + std::lock_guard lck(participant_info->entity_creation_mutex_); + + // Get RMW Subscriber + auto info = static_cast(subscription->data); + + // Delete DataReader + ReturnCode_t ret = participant_info->subscriber_->delete_datareader(info->data_reader_); + if (ReturnCode_t::RETCODE_OK != ret) { + RMW_SET_ERROR_MSG("Failed to delete datareader"); + // This is the first failure on this function, and we have not changed state. + // This means it should be safe to return an error + return RMW_RET_ERROR; + } + + // Delete ContentFilteredTopic + if (nullptr != info->filtered_topic_) { + participant_info->participant_->delete_contentfilteredtopic(info->filtered_topic_); + info->filtered_topic_ = nullptr; + } + + if (reset_cft) { + return RMW_RET_OK; + } + + // Delete DataReader listener + delete info->listener_; + + // Delete topic and unregister type + remove_topic_and_type(participant_info, info->topic_, info->type_support_); + + // Delete CustomSubscriberInfo structure + delete info; + } + + rmw_free(const_cast(subscription->topic_name)); + rmw_subscription_free(subscription); + + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_ERROR); // on completion + return RMW_RET_OK; +} +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/time_utils.cpp b/src/time_utils.cpp new file mode 100644 index 0000000..2c534e7 --- /dev/null +++ b/src/time_utils.cpp @@ -0,0 +1,35 @@ +// 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_dds_common/time_utils.hpp" + +#include "time_utils.hpp" + +namespace rmw_fastrtps_shared_cpp +{ + +eprosima::fastrtps::Duration_t +rmw_time_to_fastrtps(const rmw_time_t & time) +{ + if (rmw_time_equal(time, RMW_DURATION_INFINITE)) { + return eprosima::fastrtps::rtps::c_RTPSTimeInfinite.to_duration_t(); + } + + rmw_time_t clamped_time = rmw_dds_common::clamp_rmw_time_to_dds_time(time); + return eprosima::fastrtps::Duration_t( + static_cast(clamped_time.sec), + static_cast(clamped_time.nsec)); +} + +} // namespace rmw_fastrtps_shared_cpp diff --git a/src/time_utils.hpp b/src/time_utils.hpp new file mode 100644 index 0000000..f22a37c --- /dev/null +++ b/src/time_utils.hpp @@ -0,0 +1,27 @@ +// 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. + +#ifndef TIME_UTILS_HPP_ +#define TIME_UTILS_HPP_ + +#include "fastdds/rtps/common/Time_t.h" + +namespace rmw_fastrtps_shared_cpp +{ + +eprosima::fastrtps::Duration_t rmw_time_to_fastrtps(const rmw_time_t & time); + +} // namespace rmw_fastrtps_shared_cpp + +#endif // TIME_UTILS_HPP_ diff --git a/src/types/custom_wait_set_info.hpp b/src/types/custom_wait_set_info.hpp new file mode 100644 index 0000000..1ebdd57 --- /dev/null +++ b/src/types/custom_wait_set_info.hpp @@ -0,0 +1,27 @@ +// 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 TYPES__CUSTOM_WAIT_SET_INFO_HPP_ +#define TYPES__CUSTOM_WAIT_SET_INFO_HPP_ + +#include +#include + +typedef struct CustomWaitsetInfo +{ + std::condition_variable condition; + std::mutex condition_mutex; +} CustomWaitsetInfo; + +#endif // TYPES__CUSTOM_WAIT_SET_INFO_HPP_ diff --git a/src/types/event_types.hpp b/src/types/event_types.hpp new file mode 100644 index 0000000..8eb9d68 --- /dev/null +++ b/src/types/event_types.hpp @@ -0,0 +1,30 @@ +// 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. + +#ifndef TYPES__EVENT_TYPES_HPP_ +#define TYPES__EVENT_TYPES_HPP_ + +#include "rmw/event.h" + +namespace rmw_fastrtps_shared_cpp +{ +namespace internal +{ + +bool is_event_supported(rmw_event_type_t event_type); + +} // namespace internal +} // namespace rmw_fastrtps_shared_cpp + +#endif // TYPES__EVENT_TYPES_HPP_ diff --git a/src/types/guard_condition.hpp b/src/types/guard_condition.hpp new file mode 100644 index 0000000..ab50765 --- /dev/null +++ b/src/types/guard_condition.hpp @@ -0,0 +1,87 @@ +// 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 TYPES__GUARD_CONDITION_HPP_ +#define TYPES__GUARD_CONDITION_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "rcpputils/thread_safety_annotations.hpp" + +class GuardCondition +{ +public: + GuardCondition() + : hasTriggered_(false), + conditionMutex_(nullptr), conditionVariable_(nullptr) {} + + void + trigger() + { + std::lock_guard lock(internalMutex_); + + if (conditionMutex_ != nullptr) { + std::unique_lock clock(*conditionMutex_); + // the change to hasTriggered_ needs to be mutually exclusive with + // rmw_wait() which checks hasTriggered() and decides if wait() needs to + // be called + hasTriggered_ = true; + clock.unlock(); + conditionVariable_->notify_one(); + } else { + hasTriggered_ = true; + } + } + + void + attachCondition(std::mutex * conditionMutex, std::condition_variable * conditionVariable) + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = conditionMutex; + conditionVariable_ = conditionVariable; + } + + void + detachCondition() + { + std::lock_guard lock(internalMutex_); + conditionMutex_ = nullptr; + conditionVariable_ = nullptr; + } + + bool + hasTriggered() + { + return hasTriggered_; + } + + bool + getHasTriggered() + { + return hasTriggered_.exchange(false); + } + +private: + std::mutex internalMutex_; + std::atomic_bool hasTriggered_; + std::mutex * conditionMutex_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); + std::condition_variable * conditionVariable_ RCPPUTILS_TSA_GUARDED_BY(internalMutex_); +}; + +#endif // TYPES__GUARD_CONDITION_HPP_ diff --git a/src/utils.cpp b/src/utils.cpp new file mode 100644 index 0000000..f652abe --- /dev/null +++ b/src/utils.cpp @@ -0,0 +1,212 @@ +// Copyright 2021 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_fastrtps_shared_cpp/utils.hpp" + +#include "fastdds/dds/topic/Topic.hpp" +#include "fastdds/dds/topic/TopicDescription.hpp" +#include "fastdds/dds/topic/TypeSupport.hpp" + +#include "fastrtps/types/TypesBase.h" + +#include "rmw/rmw.h" + +using ReturnCode_t = eprosima::fastrtps::types::ReturnCode_t; + +const char * const CONTENT_FILTERED_TOPIC_POSTFIX = "_filtered_name"; + +namespace rmw_fastrtps_shared_cpp +{ + +rmw_ret_t cast_error_dds_to_rmw(ReturnCode_t code) +{ + // not switch because it is not an enum class + if (ReturnCode_t::RETCODE_OK == code) { + return RMW_RET_OK; + } else if (ReturnCode_t::RETCODE_ERROR == code) { + // repeats the error to avoid too many 'if' comparisons + return RMW_RET_ERROR; + } else if (ReturnCode_t::RETCODE_TIMEOUT == code) { + return RMW_RET_TIMEOUT; + } else if (ReturnCode_t::RETCODE_UNSUPPORTED == code) { + return RMW_RET_UNSUPPORTED; + } else if (ReturnCode_t::RETCODE_BAD_PARAMETER == code) { + return RMW_RET_INVALID_ARGUMENT; + } else if (ReturnCode_t::RETCODE_OUT_OF_RESOURCES == code) { + // Could be that out of resources comes from a different source than a bad allocation + return RMW_RET_BAD_ALLOC; + } else { + return RMW_RET_ERROR; + } +} + +bool +cast_or_create_topic( + eprosima::fastdds::dds::DomainParticipant * participant, + eprosima::fastdds::dds::TopicDescription * desc, + const std::string & topic_name, + const std::string & type_name, + const eprosima::fastdds::dds::TopicQos & topic_qos, + bool is_writer_topic, + TopicHolder * topic_holder) +{ + topic_holder->should_be_deleted = false; + topic_holder->participant = participant; + topic_holder->desc = desc; + topic_holder->topic = nullptr; + + if (nullptr == desc) { + topic_holder->topic = participant->create_topic( + topic_name, + type_name, + topic_qos); + + if (!topic_holder->topic) { + return false; + } + + topic_holder->desc = topic_holder->topic; + topic_holder->should_be_deleted = true; + } else { + if (is_writer_topic) { + topic_holder->topic = dynamic_cast(desc); + assert(nullptr != topic_holder->topic); + } + } + + return true; +} + +bool +find_and_check_topic_and_type( + const CustomParticipantInfo * participant_info, + const std::string & topic_name, + const std::string & type_name, + eprosima::fastdds::dds::TopicDescription ** returned_topic, + eprosima::fastdds::dds::TypeSupport * returned_type) +{ + // Searchs for an already existing topic + *returned_topic = participant_info->participant_->lookup_topicdescription(topic_name); + if (nullptr != *returned_topic) { + if ((*returned_topic)->get_type_name() != type_name) { + return false; + } + } + + *returned_type = participant_info->participant_->find_type(type_name); + return true; +} + +void +remove_topic_and_type( + const CustomParticipantInfo * participant_info, + const eprosima::fastdds::dds::TopicDescription * topic_desc, + const eprosima::fastdds::dds::TypeSupport & type) +{ + // TODO(MiguelCompany): We only create Topic instances at the moment, but this may + // change in the future if we start supporting other kinds of TopicDescription + // (like ContentFilteredTopic) + auto topic = dynamic_cast(topic_desc); + + if (nullptr != topic) { + participant_info->participant_->delete_topic(topic); + } + + if (type) { + participant_info->participant_->unregister_type(type.get_type_name()); + } +} + +bool +create_content_filtered_topic( + eprosima::fastdds::dds::DomainParticipant * participant, + eprosima::fastdds::dds::TopicDescription * topic_desc, + const std::string & topic_name_mangled, + const rmw_subscription_content_filter_options_t * options, + eprosima::fastdds::dds::ContentFilteredTopic ** content_filtered_topic) +{ + std::vector expression_parameters; + for (size_t i = 0; i < options->expression_parameters.size; ++i) { + expression_parameters.push_back(options->expression_parameters.data[i]); + } + + auto topic = dynamic_cast(topic_desc); + std::string cft_topic_name = topic_name_mangled + CONTENT_FILTERED_TOPIC_POSTFIX; + eprosima::fastdds::dds::ContentFilteredTopic * filtered_topic = + participant->create_contentfilteredtopic( + cft_topic_name, + topic, + options->filter_expression, + expression_parameters); + if (filtered_topic == nullptr) { + return false; + } + + *content_filtered_topic = filtered_topic; + return true; +} + +bool +create_datareader( + const eprosima::fastdds::dds::DataReaderQos & datareader_qos, + const rmw_subscription_options_t * subscription_options, + eprosima::fastdds::dds::Subscriber * subscriber, + eprosima::fastdds::dds::TopicDescription * des_topic, + SubListener * listener, + eprosima::fastdds::dds::DataReader ** data_reader +) +{ + eprosima::fastdds::dds::DataReaderQos updated_qos = datareader_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 + using PropertyPolicyHelper = eprosima::fastrtps::rtps::PropertyPolicyHelper; + if (nullptr == + PropertyPolicyHelper::find_property( + updated_qos.properties(), + "fastdds.unique_network_flows")) + { + updated_qos.properties().properties().emplace_back("fastdds.unique_network_flows", ""); + } + break; + } + + // Creates DataReader (with subscriber name to not change name policy) + *data_reader = subscriber->create_datareader( + des_topic, + updated_qos, + listener); + if (!data_reader && + (RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED == + subscription_options->require_unique_network_flow_endpoints)) + { + *data_reader = subscriber->create_datareader( + des_topic, + datareader_qos, + listener); + } + return true; +} + + +} // namespace rmw_fastrtps_shared_cpp diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..66478cd --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,55 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(osrf_testing_tools_cpp REQUIRED) + +ament_add_gtest(test_dds_attributes_to_rmw_qos test_dds_attributes_to_rmw_qos.cpp) +if(TARGET test_dds_attributes_to_rmw_qos) + ament_target_dependencies(test_dds_attributes_to_rmw_qos) + target_link_libraries(test_dds_attributes_to_rmw_qos ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_dds_qos_to_rmw_qos test_dds_qos_to_rmw_qos.cpp) +if(TARGET test_dds_qos_to_rmw_qos) + ament_target_dependencies(test_dds_qos_to_rmw_qos) + target_link_libraries(test_dds_qos_to_rmw_qos ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_rmw_qos_to_dds_attributes test_rmw_qos_to_dds_attributes.cpp) +if(TARGET test_rmw_qos_to_dds_attributes) + target_link_libraries(test_rmw_qos_to_dds_attributes ${PROJECT_NAME}) +endif() + +ament_add_gmock(test_security_logging test_security_logging.cpp) +if(TARGET test_security_logging) + ament_target_dependencies(test_security_logging) + target_link_libraries(test_security_logging ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_rmw_init_options test_rmw_init_options.cpp) +if(TARGET test_rmw_init_options) + ament_target_dependencies(test_rmw_init_options osrf_testing_tools_cpp rcutils rmw) + target_link_libraries(test_rmw_init_options ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_guid_utils test_guid_utils.cpp) +if(TARGET test_guid_utils) + target_link_libraries(test_guid_utils ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_names test_names.cpp) +if(TARGET test_names) + ament_target_dependencies(test_names rmw) + target_link_libraries(test_names ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_qos_profile_check_compatible test_qos_profile_check_compatible.cpp) +if(TARGET test_qos_profile_check_compatible) + ament_target_dependencies(test_qos_profile_check_compatible rmw) + target_link_libraries(test_qos_profile_check_compatible ${PROJECT_NAME}) +endif() + +ament_add_gtest(test_logging test_logging.cpp) +if(TARGET test_logging) + ament_target_dependencies(test_logging + osrf_testing_tools_cpp rcutils rmw) + target_link_libraries(test_logging rmw_fastrtps_shared_cpp) +endif() diff --git a/test/test_dds_attributes_to_rmw_qos.cpp b/test/test_dds_attributes_to_rmw_qos.cpp new file mode 100644 index 0000000..9171686 --- /dev/null +++ b/test/test_dds_attributes_to_rmw_qos.cpp @@ -0,0 +1,160 @@ +// 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 + +#include "gtest/gtest.h" + +#include "fastrtps/attributes/PublisherAttributes.h" +#include "fastrtps/attributes/SubscriberAttributes.h" + +#include "rmw_fastrtps_shared_cpp/qos.hpp" + +using eprosima::fastrtps::PublisherAttributes; +using eprosima::fastrtps::SubscriberAttributes; +static const eprosima::fastrtps::Duration_t InfiniteDuration = + eprosima::fastrtps::rtps::c_RTPSTimeInfinite.to_duration_t(); + +class DDSAttributesToRMWQosTest : public ::testing::Test +{ +protected: + rmw_qos_profile_t qos_profile_ {}; + PublisherAttributes publisher_attributes_ {}; + SubscriberAttributes subscriber_attributes_ {}; +}; + + +TEST_F(DDSAttributesToRMWQosTest, test_publisher_depth_conversion) { + publisher_attributes_.topic.historyQos.depth = 0; + dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.depth, 0u); +} + +TEST_F(DDSAttributesToRMWQosTest, test_publisher_history_conversion) { + publisher_attributes_.topic.historyQos.kind = eprosima::fastrtps::KEEP_ALL_HISTORY_QOS; + dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.history, RMW_QOS_POLICY_HISTORY_KEEP_ALL); +} + +TEST_F(DDSAttributesToRMWQosTest, test_publisher_durability_conversion) { + publisher_attributes_.qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_DURABILITY_QOS; + dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.durability, RMW_QOS_POLICY_DURABILITY_UNKNOWN); +} + +TEST_F(DDSAttributesToRMWQosTest, test_publisher_reliability_conversion) { + publisher_attributes_.qos.m_reliability.kind = eprosima::fastrtps::RELIABLE_RELIABILITY_QOS; + dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); +} + +TEST_F(DDSAttributesToRMWQosTest, test_publisher_liveliness_conversion) { + publisher_attributes_.qos.m_liveliness.kind = eprosima::fastrtps::MANUAL_BY_TOPIC_LIVELINESS_QOS; + dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.liveliness, RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); +} + +TEST_F(DDSAttributesToRMWQosTest, test_publisher_liveliness_lease_duration_conversion) { + publisher_attributes_.qos.m_liveliness.lease_duration = {8, 78901234}; + dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.liveliness_lease_duration.sec, 8u); + EXPECT_EQ(qos_profile_.liveliness_lease_duration.nsec, 78901234u); +} + +TEST_F(DDSAttributesToRMWQosTest, test_publisher_deadline_conversion) { + publisher_attributes_.qos.m_deadline.period = {12, 1234}; + dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.deadline.sec, 12u); + EXPECT_EQ(qos_profile_.deadline.nsec, 1234u); +} + +TEST_F(DDSAttributesToRMWQosTest, test_publisher_lifespan_conversion) { + publisher_attributes_.qos.m_lifespan.duration = {19, 5432}; + dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.lifespan.sec, 19u); + EXPECT_EQ(qos_profile_.lifespan.nsec, 5432u); +} + + +TEST_F(DDSAttributesToRMWQosTest, test_subscriber_depth_conversion) { + subscriber_attributes_.topic.historyQos.depth = 1; + dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.depth, 1u); +} + +TEST_F(DDSAttributesToRMWQosTest, test_subscriber_history_conversion) { + subscriber_attributes_.topic.historyQos.kind = eprosima::fastrtps::KEEP_LAST_HISTORY_QOS; + dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.history, RMW_QOS_POLICY_HISTORY_KEEP_LAST); +} + +TEST_F(DDSAttributesToRMWQosTest, test_subscriber_durability_conversion) { + subscriber_attributes_.qos.m_durability.kind = eprosima::fastrtps::TRANSIENT_LOCAL_DURABILITY_QOS; + dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); +} + +TEST_F(DDSAttributesToRMWQosTest, test_subscriber_reliability_conversion) { + subscriber_attributes_.qos.m_reliability.kind = eprosima::fastrtps::BEST_EFFORT_RELIABILITY_QOS; + dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.reliability, RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); +} + +TEST_F(DDSAttributesToRMWQosTest, test_subscriber_liveliness_conversion) { + subscriber_attributes_.qos.m_liveliness.kind = + eprosima::fastrtps::MANUAL_BY_PARTICIPANT_LIVELINESS_QOS; + dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.liveliness, RMW_QOS_POLICY_LIVELINESS_UNKNOWN); +} + +TEST_F(DDSAttributesToRMWQosTest, test_subscriber_liveliness_lease_duration_conversion) { + subscriber_attributes_.qos.m_liveliness.lease_duration = {80, 34567}; + dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.liveliness_lease_duration.sec, 80u); + EXPECT_EQ(qos_profile_.liveliness_lease_duration.nsec, 34567u); +} + +TEST_F(DDSAttributesToRMWQosTest, test_subscriber_deadline_conversion) { + subscriber_attributes_.qos.m_deadline.period = {1, 3324}; + dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.deadline.sec, 1u); + EXPECT_EQ(qos_profile_.deadline.nsec, 3324u); +} + +TEST_F(DDSAttributesToRMWQosTest, test_subscriber_lifespan_conversion) { + subscriber_attributes_.qos.m_lifespan.duration = {9, 432}; + dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); + EXPECT_EQ(qos_profile_.lifespan.sec, 9u); + EXPECT_EQ(qos_profile_.lifespan.nsec, 432u); +} + +TEST_F(DDSAttributesToRMWQosTest, test_subscriber_infinite_duration_conversions) { + subscriber_attributes_.qos.m_lifespan.duration = InfiniteDuration; + subscriber_attributes_.qos.m_deadline.period = InfiniteDuration; + subscriber_attributes_.qos.m_liveliness.lease_duration = InfiniteDuration; + dds_attributes_to_rmw_qos(subscriber_attributes_, &qos_profile_); + EXPECT_TRUE(rmw_time_equal(qos_profile_.deadline, RMW_DURATION_INFINITE)); + EXPECT_TRUE(rmw_time_equal(qos_profile_.lifespan, RMW_DURATION_INFINITE)); + EXPECT_TRUE(rmw_time_equal(qos_profile_.liveliness_lease_duration, RMW_DURATION_INFINITE)); +} + +TEST_F(DDSAttributesToRMWQosTest, test_publisher_infinite_duration_conversions) { + publisher_attributes_.qos.m_lifespan.duration = InfiniteDuration; + publisher_attributes_.qos.m_deadline.period = InfiniteDuration; + publisher_attributes_.qos.m_liveliness.lease_duration = InfiniteDuration; + dds_attributes_to_rmw_qos(publisher_attributes_, &qos_profile_); + EXPECT_TRUE(rmw_time_equal(qos_profile_.deadline, RMW_DURATION_INFINITE)); + EXPECT_TRUE(rmw_time_equal(qos_profile_.lifespan, RMW_DURATION_INFINITE)); + EXPECT_TRUE(rmw_time_equal(qos_profile_.liveliness_lease_duration, RMW_DURATION_INFINITE)); +} diff --git a/test/test_dds_qos_to_rmw_qos.cpp b/test/test_dds_qos_to_rmw_qos.cpp new file mode 100644 index 0000000..88aef4e --- /dev/null +++ b/test/test_dds_qos_to_rmw_qos.cpp @@ -0,0 +1,161 @@ +// Copyright 2021 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 "gtest/gtest.h" + +#include "fastdds/dds/core/policy/QosPolicies.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" + +#include "rmw_fastrtps_shared_cpp/qos.hpp" + +using eprosima::fastdds::dds::DataReaderQos; +using eprosima::fastdds::dds::DataWriterQos; +static const eprosima::fastrtps::Duration_t InfiniteDuration = + eprosima::fastrtps::rtps::c_RTPSTimeInfinite.to_duration_t(); + +class DDSQosToRMWQosTest : public ::testing::Test +{ +protected: + rmw_qos_profile_t qos_profile_ {}; + DataWriterQos writer_qos_ {}; + DataReaderQos reader_qos_ {}; +}; + + +TEST_F(DDSQosToRMWQosTest, test_publisher_depth_conversion) { + writer_qos_.history().depth = 0; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.depth, 0u); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_history_conversion) { + writer_qos_.history().kind = eprosima::fastdds::dds::KEEP_ALL_HISTORY_QOS; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.history, RMW_QOS_POLICY_HISTORY_KEEP_ALL); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_durability_conversion) { + writer_qos_.durability().kind = eprosima::fastdds::dds::TRANSIENT_DURABILITY_QOS; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.durability, RMW_QOS_POLICY_DURABILITY_UNKNOWN); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_reliability_conversion) { + writer_qos_.reliability().kind = eprosima::fastdds::dds::RELIABLE_RELIABILITY_QOS; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_liveliness_conversion) { + writer_qos_.liveliness().kind = eprosima::fastdds::dds::MANUAL_BY_TOPIC_LIVELINESS_QOS; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.liveliness, RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_liveliness_lease_duration_conversion) { + writer_qos_.liveliness().lease_duration = {8, 78901234}; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.liveliness_lease_duration.sec, 8u); + EXPECT_EQ(qos_profile_.liveliness_lease_duration.nsec, 78901234u); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_deadline_conversion) { + writer_qos_.deadline().period = {12, 1234}; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.deadline.sec, 12u); + EXPECT_EQ(qos_profile_.deadline.nsec, 1234u); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_lifespan_conversion) { + writer_qos_.lifespan().duration = {19, 5432}; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.lifespan.sec, 19u); + EXPECT_EQ(qos_profile_.lifespan.nsec, 5432u); +} + + +TEST_F(DDSQosToRMWQosTest, test_subscriber_depth_conversion) { + reader_qos_.history().depth = 1; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.depth, 1u); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_history_conversion) { + reader_qos_.history().kind = eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.history, RMW_QOS_POLICY_HISTORY_KEEP_LAST); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_durability_conversion) { + reader_qos_.durability().kind = eprosima::fastdds::dds::TRANSIENT_LOCAL_DURABILITY_QOS; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_reliability_conversion) { + reader_qos_.reliability().kind = eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.reliability, RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_liveliness_conversion) { + reader_qos_.liveliness().kind = + eprosima::fastdds::dds::MANUAL_BY_PARTICIPANT_LIVELINESS_QOS; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.liveliness, RMW_QOS_POLICY_LIVELINESS_UNKNOWN); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_liveliness_lease_duration_conversion) { + reader_qos_.liveliness().lease_duration = {80, 34567}; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.liveliness_lease_duration.sec, 80u); + EXPECT_EQ(qos_profile_.liveliness_lease_duration.nsec, 34567u); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_deadline_conversion) { + reader_qos_.deadline().period = {1, 3324}; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.deadline.sec, 1u); + EXPECT_EQ(qos_profile_.deadline.nsec, 3324u); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_lifespan_conversion) { + reader_qos_.lifespan().duration = {9, 432}; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_EQ(qos_profile_.lifespan.sec, 9u); + EXPECT_EQ(qos_profile_.lifespan.nsec, 432u); +} + +TEST_F(DDSQosToRMWQosTest, test_subscriber_infinite_duration_conversions) { + reader_qos_.lifespan().duration = InfiniteDuration; + reader_qos_.deadline().period = InfiniteDuration; + reader_qos_.liveliness().lease_duration = InfiniteDuration; + dds_qos_to_rmw_qos(reader_qos_, &qos_profile_); + EXPECT_TRUE(rmw_time_equal(qos_profile_.deadline, RMW_DURATION_INFINITE)); + EXPECT_TRUE(rmw_time_equal(qos_profile_.lifespan, RMW_DURATION_INFINITE)); + EXPECT_TRUE(rmw_time_equal(qos_profile_.liveliness_lease_duration, RMW_DURATION_INFINITE)); +} + +TEST_F(DDSQosToRMWQosTest, test_publisher_infinite_duration_conversions) { + writer_qos_.lifespan().duration = InfiniteDuration; + writer_qos_.deadline().period = InfiniteDuration; + writer_qos_.liveliness().lease_duration = InfiniteDuration; + dds_qos_to_rmw_qos(writer_qos_, &qos_profile_); + EXPECT_TRUE(rmw_time_equal(qos_profile_.deadline, RMW_DURATION_INFINITE)); + EXPECT_TRUE(rmw_time_equal(qos_profile_.lifespan, RMW_DURATION_INFINITE)); + EXPECT_TRUE(rmw_time_equal(qos_profile_.liveliness_lease_duration, RMW_DURATION_INFINITE)); +} diff --git a/test/test_guid_utils.cpp b/test/test_guid_utils.cpp new file mode 100644 index 0000000..3bba2a2 --- /dev/null +++ b/test/test_guid_utils.cpp @@ -0,0 +1,62 @@ +// 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/rtps/common/EntityId_t.hpp" +#include "fastdds/rtps/common/Guid.h" +#include "fastdds/rtps/common/GuidPrefix_t.hpp" + +#include "rmw_fastrtps_shared_cpp/guid_utils.hpp" + +using rmw_fastrtps_shared_cpp::copy_from_byte_array_to_fastrtps_guid; +using rmw_fastrtps_shared_cpp::copy_from_fastrtps_guid_to_byte_array; + +static constexpr size_t byte_array_size = + eprosima::fastrtps::rtps::GuidPrefix_t::size + + eprosima::fastrtps::rtps::EntityId_t::size; + +TEST(GUIDUtilsTest, bad_arguments) { +#ifndef NDEBUG + eprosima::fastrtps::rtps::GUID_t guid; + uint8_t byte_array[byte_array_size] = {0}; + uint8_t * null_byte_array = nullptr; + EXPECT_DEATH(copy_from_byte_array_to_fastrtps_guid(byte_array, nullptr), ""); + EXPECT_DEATH(copy_from_byte_array_to_fastrtps_guid(null_byte_array, &guid), ""); + EXPECT_DEATH(copy_from_fastrtps_guid_to_byte_array(guid, null_byte_array), ""); +#endif +} + +TEST(GUIDUtilsTest, byte_array_to_guid_and_back) { + uint8_t input_byte_array[byte_array_size] = {0}; + input_byte_array[0] = 0xA5; + input_byte_array[byte_array_size - 1] = 0x4B; + eprosima::fastrtps::rtps::GUID_t guid; + copy_from_byte_array_to_fastrtps_guid(input_byte_array, &guid); + uint8_t output_byte_array[byte_array_size] = {0}; + copy_from_fastrtps_guid_to_byte_array(guid, output_byte_array); + EXPECT_EQ(0, memcmp(input_byte_array, output_byte_array, byte_array_size)); +} + +TEST(GUIDUtilsTest, guid_to_byte_array_and_back) { + eprosima::fastrtps::rtps::GuidPrefix_t prefix; + prefix.value[0] = 0xD2; + prefix.value[eprosima::fastrtps::rtps::GuidPrefix_t::size - 1] = 0x3E; + eprosima::fastrtps::rtps::GUID_t input_guid{prefix, 1234}; + uint8_t byte_array[byte_array_size] = {0}; + copy_from_fastrtps_guid_to_byte_array(input_guid, byte_array); + eprosima::fastrtps::rtps::GUID_t output_guid; + copy_from_byte_array_to_fastrtps_guid(byte_array, &output_guid); + EXPECT_EQ(input_guid, output_guid); +} diff --git a/test/test_logging.cpp b/test/test_logging.cpp new file mode 100644 index 0000000..93969fa --- /dev/null +++ b/test/test_logging.cpp @@ -0,0 +1,50 @@ +// 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" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +TEST(TestLogging, rmw_logging) +{ + using eprosima::fastdds::dds::Log; + + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_DEBUG); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); + ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_INFO); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(Log::Kind::Info, Log::GetVerbosity()); + ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_WARN); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(Log::Kind::Warning, Log::GetVerbosity()); + ret = rmw_fastrtps_shared_cpp::__rmw_set_log_severity(RMW_LOG_SEVERITY_ERROR); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(Log::Kind::Error, Log::GetVerbosity()); + ret = rmw_fastrtps_shared_cpp::__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_fastrtps_shared_cpp::__rmw_set_log_severity( + static_cast(RMW_LOG_SEVERITY_FATAL + 1)); + EXPECT_EQ(ret, RMW_RET_ERROR); +} diff --git a/test/test_names.cpp b/test/test_names.cpp new file mode 100644 index 0000000..9f30c99 --- /dev/null +++ b/test/test_names.cpp @@ -0,0 +1,92 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" + +#include "rmw/qos_profiles.h" + +#include "rmw_fastrtps_shared_cpp/names.hpp" +#include "rmw_fastrtps_shared_cpp/namespace_prefix.hpp" + +TEST(NamespaceTest, get_prefix) { + EXPECT_EQ("", _get_ros_prefix_if_exists("")); + EXPECT_EQ("", _get_ros_prefix_if_exists("not/a_ros_prefix")); + for (const auto & prefix : _get_all_ros_prefixes()) { + EXPECT_EQ("", _get_ros_prefix_if_exists(prefix)); + EXPECT_EQ("", _get_ros_prefix_if_exists(prefix + "_should_not_match")); + EXPECT_EQ("", _get_ros_prefix_if_exists("th/is_should_not_match/" + prefix)); + EXPECT_EQ(prefix, _get_ros_prefix_if_exists(prefix + "/")); + EXPECT_EQ(prefix, _get_ros_prefix_if_exists(prefix + "/should_match")); + } +} + +TEST(NamespaceTest, strip_prefix) { + EXPECT_EQ("", _strip_ros_prefix_if_exists("")); + EXPECT_EQ("/no_ros_prefix/test", _strip_ros_prefix_if_exists("/no_ros_prefix/test")); + for (const auto & prefix : _get_all_ros_prefixes()) { + EXPECT_EQ(prefix, _strip_ros_prefix_if_exists(prefix)); + EXPECT_EQ("/", _strip_ros_prefix_if_exists(prefix + "/")); + EXPECT_EQ( + prefix + "should_not_be_stripped", + _strip_ros_prefix_if_exists(prefix + "should_not_be_stripped")); + EXPECT_EQ( + "th/is_should_not_be_stripped/" + prefix, + _strip_ros_prefix_if_exists("th/is_should_not_be_stripped/" + prefix)); + EXPECT_EQ("/should_be_stripped", _strip_ros_prefix_if_exists(prefix + "/should_be_stripped")); + } +} + +TEST(NamespaceTest, resolve_prefix) { + EXPECT_EQ("", _resolve_prefix("", "")); + EXPECT_EQ("", _resolve_prefix("", "some_ros_prefix")); + EXPECT_EQ("", _resolve_prefix("/test", "some_ros_prefix")); + EXPECT_EQ("/test", _resolve_prefix("/test", "")); + EXPECT_EQ("", _resolve_prefix("some_ros_prefix", "some_ros_prefix")); + EXPECT_EQ("/", _resolve_prefix("some_ros_prefix/", "some_ros_prefix")); + EXPECT_EQ( + "/test_some_ros_prefix", + _resolve_prefix("some_ros_prefix/test_some_ros_prefix", "some_ros_prefix")); + EXPECT_EQ( + "", _resolve_prefix("some_ros_prefix_test", "some_ros_prefix")); + EXPECT_EQ( + "", _resolve_prefix("this_ros_prefix/test/some_ros_prefix", "some_ros_prefix")); +} + +TEST(NamespaceTest, name_mangling) { + rmw_qos_profile_t qos_profile = rmw_qos_profile_unknown; + qos_profile.avoid_ros_namespace_conventions = false; + +#ifndef NDEBUG + EXPECT_DEATH(_create_topic_name(nullptr, "", "", ""), ""); + + EXPECT_DEATH(_create_topic_name(&qos_profile, "", nullptr, ""), ""); +#endif + + EXPECT_STREQ( + "some_ros_prefix/test__suffix", _create_topic_name( + &qos_profile, "some_ros_prefix", "/test", "__suffix").c_str()); + + EXPECT_STREQ( + "/test__suffix", _create_topic_name( + &qos_profile, nullptr, "/test", "__suffix").c_str()); + + EXPECT_STREQ( + "some_ros_prefix/test", _create_topic_name( + &qos_profile, "some_ros_prefix", "/test", nullptr).c_str()); + + qos_profile.avoid_ros_namespace_conventions = true; + EXPECT_STREQ( + "/test__suffix", _create_topic_name( + &qos_profile, "some_ros_prefix", "/test", "__suffix").c_str()); +} diff --git a/test/test_qos_profile_check_compatible.cpp b/test/test_qos_profile_check_compatible.cpp new file mode 100644 index 0000000..0fbfeb1 --- /dev/null +++ b/test/test_qos_profile_check_compatible.cpp @@ -0,0 +1,101 @@ +// 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 + +#include "rmw/qos_profiles.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_common.hpp" + +TEST(TestQoSProfileCheckCompatible, compatible) +{ + rmw_qos_profile_t qos_profile = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 5, // history depth + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + {1, 0}, // deadline + {1, 0}, // lifespan + RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, + {1, 0}, // liveliness lease duration + false // avoid_ros_namespace_conventions + }; + + rmw_qos_compatibility_type_t compatibility; + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_qos_profile_check_compatible( + qos_profile, + qos_profile, + &compatibility, + nullptr, + 0u); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(compatibility, RMW_QOS_COMPATIBILITY_OK); +} + +TEST(TestQoSProfileCheckCompatible, incompatible) +{ + rmw_qos_profile_t pub_profile = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 5, // history depth + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + {1, 0}, // deadline + {1, 0}, // lifespan + RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, + {1, 0}, // liveliness lease duration + false // avoid_ros_namespace_conventions + }; + + rmw_qos_profile_t sub_profile = pub_profile; + sub_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + rmw_qos_compatibility_type_t compatibility; + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_qos_profile_check_compatible( + pub_profile, + sub_profile, + &compatibility, + nullptr, + 0u); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(compatibility, RMW_QOS_COMPATIBILITY_ERROR); +} + +TEST(TestQoSProfileCheckCompatible, warn_compatible) +{ + rmw_qos_profile_t pub_profile = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 5, // history depth + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + RMW_QOS_POLICY_DURABILITY_VOLATILE, + {1, 0}, // deadline + {1, 0}, // lifespan + RMW_QOS_POLICY_LIVELINESS_AUTOMATIC, + {1, 0}, // liveliness lease duration + false // avoid_ros_namespace_conventions + }; + + rmw_qos_profile_t sub_profile = pub_profile; + sub_profile.reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; + + rmw_qos_compatibility_type_t compatibility; + rmw_ret_t ret = rmw_fastrtps_shared_cpp::__rmw_qos_profile_check_compatible( + pub_profile, + sub_profile, + &compatibility, + nullptr, + 0u); + EXPECT_EQ(ret, RMW_RET_OK); + EXPECT_EQ(compatibility, RMW_QOS_COMPATIBILITY_WARNING); +} diff --git a/test/test_rmw_init_options.cpp b/test/test_rmw_init_options.cpp new file mode 100644 index 0000000..a2aa790 --- /dev/null +++ b/test/test_rmw_init_options.cpp @@ -0,0 +1,242 @@ +// 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 "rcutils/allocator.h" +#include "rcutils/strdup.h" +#include "rmw/init_options.h" +#include "rmw/security_options.h" +#include "rmw/types.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" + +#include "rmw_fastrtps_shared_cpp/rmw_init.hpp" + +using rmw_fastrtps_shared_cpp::rmw_init_options_init; +using rmw_fastrtps_shared_cpp::rmw_init_options_copy; +using rmw_fastrtps_shared_cpp::rmw_init_options_fini; + +static const char * const some_identifier = "some_identifier"; + +TEST(RMWInitOptionsTest, init_w_invalid_args_fails) { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + // Cannot initialize a null options instance. + EXPECT_EQ( + RMW_RET_INVALID_ARGUMENT, + rmw_init_options_init(some_identifier, nullptr, allocator)); + rcutils_reset_error(); + + rmw_init_options_t options = rmw_get_zero_initialized_init_options(); + rcutils_allocator_t invalid_allocator = rcutils_get_zero_initialized_allocator(); + // Cannot initialize using an invalid allocator. + EXPECT_EQ( + RMW_RET_INVALID_ARGUMENT, + rmw_init_options_init(some_identifier, &options, invalid_allocator)); + rcutils_reset_error(); +} + + +TEST(RMWInitOptionsTest, init_twice_fails) { + rmw_init_options_t options = rmw_get_zero_initialized_init_options(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + ASSERT_EQ(RMW_RET_OK, rmw_init_options_init(some_identifier, &options, allocator)) << + rcutils_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcutils_reset_error(); + EXPECT_EQ(RMW_RET_OK, rmw_init_options_fini(some_identifier, &options)) << + rcutils_get_error_string().str; + rcutils_reset_error(); + }); + + EXPECT_EQ( + RMW_RET_INVALID_ARGUMENT, + rmw_init_options_init(some_identifier, &options, allocator)); + rcutils_reset_error(); +} + + +TEST(RMWInitOptionsTest, init) { + rmw_init_options_t options = rmw_get_zero_initialized_init_options(); + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + + ASSERT_EQ(RMW_RET_OK, rmw_init_options_init(some_identifier, &options, allocator)) << + rcutils_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcutils_reset_error(); + EXPECT_EQ(RMW_RET_OK, rmw_init_options_fini(some_identifier, &options)) << + rcutils_get_error_string().str; + rcutils_reset_error(); + }); + + EXPECT_EQ(some_identifier, options.implementation_identifier); +} + + +TEST(RMWInitOptionsTest, copy_w_invalid_args_fails) { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_init_options_t not_initialized_options = rmw_get_zero_initialized_init_options(); + rmw_init_options_t initialized_options = rmw_get_zero_initialized_init_options(); + + ASSERT_EQ( + RMW_RET_OK, + rmw_init_options_init( + some_identifier, + &initialized_options, + allocator)) << + rcutils_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcutils_reset_error(); + EXPECT_EQ(RMW_RET_OK, rmw_init_options_fini(some_identifier, &initialized_options)) << + rcutils_get_error_string().str; + rcutils_reset_error(); + }); + + // Cannot copy from a null options instance. + EXPECT_EQ( + RMW_RET_INVALID_ARGUMENT, + rmw_init_options_copy( + some_identifier, + nullptr, + ¬_initialized_options)); + rcutils_reset_error(); + + // Cannot copy to a null options instance. + EXPECT_EQ( + RMW_RET_INVALID_ARGUMENT, + rmw_init_options_copy( + some_identifier, + &initialized_options, + nullptr)); + rcutils_reset_error(); + + // Cannot copy an options instance if implementation identifiers do not match. + EXPECT_EQ( + RMW_RET_INCORRECT_RMW_IMPLEMENTATION, + rmw_init_options_copy( + "another_identifier", + &initialized_options, + ¬_initialized_options)); + rcutils_reset_error(); + + // Cannot copy to an already initialized options instance. + EXPECT_EQ( + RMW_RET_INVALID_ARGUMENT, rmw_init_options_copy( + some_identifier, + &initialized_options, + &initialized_options)); + rcutils_reset_error(); +} + + +TEST(RMWInitOptionsTest, copy) { + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + rmw_init_options_t preset_options = rmw_get_zero_initialized_init_options(); + + rcutils_reset_error(); + ASSERT_EQ(RMW_RET_OK, rmw_init_options_init(some_identifier, &preset_options, allocator)) << + rcutils_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcutils_reset_error(); + EXPECT_EQ(RMW_RET_OK, rmw_init_options_fini(some_identifier, &preset_options)) << + rcutils_get_error_string().str; + rcutils_reset_error(); + }); + preset_options.instance_id = 23lu; + preset_options.enclave = rcutils_strdup("/test", allocator); + ASSERT_TRUE(preset_options.enclave != nullptr); + preset_options.security_options.security_root_path = rcutils_strdup("/root", allocator); + ASSERT_TRUE(preset_options.security_options.security_root_path != nullptr); + + rmw_init_options_t options = rmw_get_zero_initialized_init_options(); + ASSERT_EQ(RMW_RET_OK, rmw_init_options_copy(some_identifier, &preset_options, &options)) << + rcutils_get_error_string().str; + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcutils_reset_error(); + EXPECT_EQ(RMW_RET_OK, rmw_init_options_fini(some_identifier, &options)) << + rcutils_get_error_string().str; + rcutils_reset_error(); + }); + EXPECT_EQ(23lu, options.instance_id); + EXPECT_STREQ("/test", options.enclave); + EXPECT_STREQ("/root", options.security_options.security_root_path); +} + +static void * failing_allocate(size_t size, void * state) +{ + (void)size; + (void)state; + return NULL; +} + +TEST(RMWInitOptionsTest, bad_alloc_on_copy) { + rcutils_allocator_t failing_allocator = rcutils_get_default_allocator(); + failing_allocator.allocate = failing_allocate; + + rmw_init_options_t preset_options = rmw_get_zero_initialized_init_options(); + ASSERT_EQ( + RMW_RET_OK, + rmw_init_options_init(some_identifier, &preset_options, failing_allocator)) << + rcutils_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcutils_reset_error(); + EXPECT_EQ(RMW_RET_OK, rmw_init_options_fini(some_identifier, &preset_options)) << + rcutils_get_error_string().str; + rcutils_reset_error(); + }); + preset_options.enclave = rcutils_strdup("/test", rcutils_get_default_allocator()); + ASSERT_TRUE(preset_options.enclave != nullptr); + + rmw_init_options_t options = rmw_get_zero_initialized_init_options(); + EXPECT_EQ( + RMW_RET_BAD_ALLOC, + rmw_init_options_copy(some_identifier, &preset_options, &options)); +} + +TEST(RMWInitOptionsTest, fini_w_invalid_args_fails) { + // Cannot finalize a null options instance. + EXPECT_EQ(RMW_RET_INVALID_ARGUMENT, rmw_init_options_fini(some_identifier, nullptr)); + rcutils_reset_error(); + + rmw_init_options_t options = rmw_get_zero_initialized_init_options(); + // Cannot finalize an options instance that has not been initialized. + EXPECT_EQ(RMW_RET_INVALID_ARGUMENT, rmw_init_options_fini(some_identifier, &options)); + rcutils_reset_error(); + + rcutils_allocator_t allocator = rcutils_get_default_allocator(); + ASSERT_EQ(RMW_RET_OK, rmw_init_options_init(some_identifier, &options, allocator)) << + rcutils_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcutils_reset_error(); + EXPECT_EQ(RMW_RET_OK, rmw_init_options_fini(some_identifier, &options)) << + rcutils_get_error_string().str; + rcutils_reset_error(); + }); + + // Cannot finalize an options instance if implementation identifiers do not match. + const char * const another_identifier = "another_identifier"; + EXPECT_EQ( + RMW_RET_INCORRECT_RMW_IMPLEMENTATION, + rmw_init_options_fini(another_identifier, &options)); + rcutils_reset_error(); +} diff --git a/test/test_rmw_qos_to_dds_attributes.cpp b/test/test_rmw_qos_to_dds_attributes.cpp new file mode 100644 index 0000000..b080a13 --- /dev/null +++ b/test/test_rmw_qos_to_dds_attributes.cpp @@ -0,0 +1,247 @@ +// 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 +#include + +#include "gtest/gtest.h" + +#include "fastdds/dds/core/policy/QosPolicies.hpp" +#include "fastdds/dds/publisher/qos/DataWriterQos.hpp" +#include "fastdds/dds/subscriber/qos/DataReaderQos.hpp" + +#include "rmw_fastrtps_shared_cpp/qos.hpp" + +#include "rmw/error_handling.h" + + +using eprosima::fastdds::dds::DataReaderQos; +static const eprosima::fastrtps::Duration_t InfiniteDuration = + eprosima::fastrtps::rtps::c_RTPSTimeInfinite.to_duration_t(); + +class GetDataReaderQoSTest : public ::testing::Test +{ +protected: + rmw_qos_profile_t qos_profile_{rmw_qos_profile_default}; + DataReaderQos subscriber_qos_{}; +}; + +TEST_F(GetDataReaderQoSTest, test_unknown_history_policy_conversion_fails) { + qos_profile_.history = RMW_QOS_POLICY_HISTORY_UNKNOWN; + EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_TRUE(rmw_error_is_set()); + rmw_reset_error(); +} + +TEST_F(GetDataReaderQoSTest, unknown_reliability_policy_conversion_fails) { + qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; + EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_TRUE(rmw_error_is_set()); + rmw_reset_error(); +} + +TEST_F(GetDataReaderQoSTest, unknown_durability_policy_conversion_fails) { + qos_profile_.durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; + EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_TRUE(rmw_error_is_set()); + rmw_reset_error(); +} + +TEST_F(GetDataReaderQoSTest, unknown_liveliness_policy_conversion_fails) { + qos_profile_.liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; + EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_TRUE(rmw_error_is_set()); + rmw_reset_error(); +} + +TEST_F(GetDataReaderQoSTest, nominal_conversion) { + qos_profile_.depth = 10u; + qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + qos_profile_.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + qos_profile_.lifespan.sec = 0u; + qos_profile_.lifespan.nsec = 500000000u; + qos_profile_.deadline.sec = 0u; + qos_profile_.deadline.nsec = 100000000u; + qos_profile_.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + qos_profile_.liveliness_lease_duration.sec = 10u; + qos_profile_.liveliness_lease_duration.nsec = 0u; + + EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); + + EXPECT_EQ( + eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, + subscriber_qos_.reliability().kind); + EXPECT_EQ( + eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS, + subscriber_qos_.durability().kind); + EXPECT_EQ( + eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS, + subscriber_qos_.liveliness().kind); + EXPECT_EQ(0, subscriber_qos_.lifespan().duration.seconds); + EXPECT_EQ(500000000u, subscriber_qos_.lifespan().duration.nanosec); + EXPECT_EQ(0, subscriber_qos_.deadline().period.seconds); + EXPECT_EQ(100000000u, subscriber_qos_.deadline().period.nanosec); + EXPECT_EQ(10, subscriber_qos_.liveliness().lease_duration.seconds); + EXPECT_EQ(0u, subscriber_qos_.liveliness().lease_duration.nanosec); + EXPECT_EQ( + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, + subscriber_qos_.history().kind); + EXPECT_GE(10, subscriber_qos_.history().depth); +} + +TEST_F(GetDataReaderQoSTest, large_depth_conversion) { + size_t depth = subscriber_qos_.history().depth + 1; + qos_profile_.depth = depth; + qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + + EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); + + EXPECT_EQ( + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, + subscriber_qos_.history().kind); + EXPECT_LE(depth, static_cast(subscriber_qos_.history().depth)); + + using depth_type = decltype(subscriber_qos_.history().depth); + constexpr size_t max_depth = static_cast(std::numeric_limits::max()); + + qos_profile_.depth = max_depth; + EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_LE(depth, static_cast(subscriber_qos_.history().depth)); + + if (max_depth < std::numeric_limits::max()) { + qos_profile_.depth = max_depth + 1; + EXPECT_FALSE(get_datareader_qos(qos_profile_, subscriber_qos_)); + } +} + +using eprosima::fastdds::dds::DataWriterQos; + +class GetDataWriterQoSTest : public ::testing::Test +{ +protected: + rmw_qos_profile_t qos_profile_{rmw_qos_profile_default}; + DataWriterQos publisher_qos_{}; +}; + +TEST_F(GetDataWriterQoSTest, test_unknown_history_policy_conversion_fails) { + qos_profile_.history = RMW_QOS_POLICY_HISTORY_UNKNOWN; + EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_TRUE(rmw_error_is_set()); + rmw_reset_error(); +} + +TEST_F(GetDataWriterQoSTest, unknown_reliability_policy_conversion_fails) { + qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_UNKNOWN; + EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_TRUE(rmw_error_is_set()); + rmw_reset_error(); +} + +TEST_F(GetDataWriterQoSTest, unknown_durability_policy_conversion_fails) { + qos_profile_.durability = RMW_QOS_POLICY_DURABILITY_UNKNOWN; + EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_TRUE(rmw_error_is_set()); + rmw_reset_error(); +} + +TEST_F(GetDataWriterQoSTest, unknown_liveliness_policy_conversion_fails) { + qos_profile_.liveliness = RMW_QOS_POLICY_LIVELINESS_UNKNOWN; + EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_TRUE(rmw_error_is_set()); + rmw_reset_error(); +} + +TEST_F(GetDataWriterQoSTest, nominal_conversion) { + qos_profile_.depth = 10u; + qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos_profile_.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + qos_profile_.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + qos_profile_.lifespan.sec = 0u; + qos_profile_.lifespan.nsec = 500000000u; + qos_profile_.deadline.sec = 0u; + qos_profile_.deadline.nsec = 100000000u; + qos_profile_.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + qos_profile_.liveliness_lease_duration.sec = 10u; + qos_profile_.liveliness_lease_duration.nsec = 0u; + + EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); + + EXPECT_EQ( + eprosima::fastdds::dds::BEST_EFFORT_RELIABILITY_QOS, + publisher_qos_.reliability().kind); + EXPECT_EQ( + eprosima::fastdds::dds::VOLATILE_DURABILITY_QOS, + publisher_qos_.durability().kind); + EXPECT_EQ( + eprosima::fastdds::dds::AUTOMATIC_LIVELINESS_QOS, + publisher_qos_.liveliness().kind); + EXPECT_EQ(0, publisher_qos_.lifespan().duration.seconds); + EXPECT_EQ(500000000u, publisher_qos_.lifespan().duration.nanosec); + EXPECT_EQ(0, publisher_qos_.deadline().period.seconds); + EXPECT_EQ(100000000u, publisher_qos_.deadline().period.nanosec); + EXPECT_EQ(10, publisher_qos_.liveliness().lease_duration.seconds); + EXPECT_EQ(0u, publisher_qos_.liveliness().lease_duration.nanosec); + EXPECT_EQ( + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, + publisher_qos_.history().kind); + EXPECT_GE(10, publisher_qos_.history().depth); +} + +TEST_F(GetDataWriterQoSTest, large_depth_conversion) { + size_t depth = publisher_qos_.history().depth + 1; + qos_profile_.depth = depth; + qos_profile_.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + + EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); + + EXPECT_EQ( + eprosima::fastdds::dds::KEEP_LAST_HISTORY_QOS, + publisher_qos_.history().kind); + EXPECT_LE(depth, static_cast(publisher_qos_.history().depth)); + + using depth_type = decltype(publisher_qos_.history().depth); + constexpr size_t max_depth = static_cast(std::numeric_limits::max()); + + qos_profile_.depth = max_depth; + EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_LE(depth, static_cast(publisher_qos_.history().depth)); + + if (max_depth < std::numeric_limits::max()) { + qos_profile_.depth = max_depth + 1; + EXPECT_FALSE(get_datawriter_qos(qos_profile_, publisher_qos_)); + } +} + +TEST_F(GetDataReaderQoSTest, infinite_duration_conversions) +{ + qos_profile_.lifespan = RMW_DURATION_INFINITE; + qos_profile_.deadline = RMW_DURATION_INFINITE; + qos_profile_.liveliness_lease_duration = RMW_DURATION_INFINITE; + EXPECT_TRUE(get_datareader_qos(qos_profile_, subscriber_qos_)); + EXPECT_EQ(subscriber_qos_.lifespan().duration, InfiniteDuration); + EXPECT_EQ(subscriber_qos_.deadline().period, InfiniteDuration); + EXPECT_EQ(subscriber_qos_.liveliness().lease_duration, InfiniteDuration); +} + +TEST_F(GetDataWriterQoSTest, infinite_duration_conversions) +{ + qos_profile_.lifespan = RMW_DURATION_INFINITE; + qos_profile_.deadline = RMW_DURATION_INFINITE; + qos_profile_.liveliness_lease_duration = RMW_DURATION_INFINITE; + EXPECT_TRUE(get_datawriter_qos(qos_profile_, publisher_qos_)); + EXPECT_EQ(publisher_qos_.lifespan().duration, InfiniteDuration); + EXPECT_EQ(publisher_qos_.deadline().period, InfiniteDuration); + EXPECT_EQ(publisher_qos_.liveliness().lease_duration, InfiniteDuration); +} diff --git a/test/test_security_logging.cpp b/test/test_security_logging.cpp new file mode 100644 index 0000000..e1188e2 --- /dev/null +++ b/test/test_security_logging.cpp @@ -0,0 +1,275 @@ +// Copyright 2020 Canonical Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "fastdds/rtps/common/Property.h" +#include "fastdds/rtps/attributes/PropertyPolicy.h" + +#include "fastrtps/config.h" +#include "rcutils/filesystem.h" +#include "rmw/error_handling.h" +#include "rmw/security_options.h" +#include "rmw/types.h" + +#include "rmw_fastrtps_shared_cpp/rmw_security_logging.hpp" + +#include "gmock/gmock.h" + +using ::testing::HasSubstr; +using ::testing::MatchesRegex; + +namespace +{ + +// Environment variable names +const char log_file_variable_name[] = "ROS_SECURITY_LOG_FILE"; +#if HAVE_SECURITY +const char log_publish_variable_name[] = "ROS_SECURITY_LOG_PUBLISH"; +const char log_verbosity_variable_name[] = "ROS_SECURITY_LOG_VERBOSITY"; + +// Logging properties +const char logging_plugin_property_name[] = "dds.sec.log.plugin"; +const char log_file_property_name[] = "dds.sec.log.builtin.DDS_LogTopic.log_file"; +const char verbosity_property_name[] = "dds.sec.log.builtin.DDS_LogTopic.logging_level"; +const char distribute_enable_property_name[] = + "dds.sec.log.builtin.DDS_LogTopic.distribute"; + +const eprosima::fastrtps::rtps::Property & lookup_property( + const eprosima::fastrtps::rtps::PropertySeq & properties, const std::string & property_name) +{ + auto iterator = std::find_if( + properties.begin(), properties.end(), + [&property_name](const eprosima::fastrtps::rtps::Property & item) -> bool { + return item.name() == property_name; + }); + + if (iterator == properties.end()) { + ADD_FAILURE() << "Expected property " << property_name << " to be in list"; + } + + return *iterator; +} + +const eprosima::fastrtps::rtps::Property & logging_plugin_property( + const eprosima::fastrtps::rtps::PropertySeq & properties) +{ + return lookup_property(properties, logging_plugin_property_name); +} + +const eprosima::fastrtps::rtps::Property & log_file_property( + const eprosima::fastrtps::rtps::PropertySeq & properties) +{ + return lookup_property(properties, log_file_property_name); +} + +const eprosima::fastrtps::rtps::Property & verbosity_property( + const eprosima::fastrtps::rtps::PropertySeq & properties) +{ + return lookup_property(properties, verbosity_property_name); +} + +const eprosima::fastrtps::rtps::Property & distribute_enable_property( + const eprosima::fastrtps::rtps::PropertySeq & properties) +{ + return lookup_property(properties, distribute_enable_property_name); +} + +#endif + +void custom_setenv(const std::string & variable_name, const std::string & value) +{ +#ifdef _WIN32 + auto ret = _putenv_s(variable_name.c_str(), value.c_str()); +#else + auto ret = setenv(variable_name.c_str(), value.c_str(), 1); +#endif + if (ret != 0) { + ADD_FAILURE() << "Unable to set environment variable: expected 0, got " << ret; + } +} + +class SecurityLoggingTest : public ::testing::Test +{ +public: + void SetUp() + { +#if HAVE_SECURITY + custom_setenv(log_file_variable_name, ""); + custom_setenv(log_publish_variable_name, ""); + custom_setenv(log_verbosity_variable_name, ""); +#endif + } + void TearDown() + { + rmw_reset_error(); + } +}; +} // namespace + +#if HAVE_SECURITY + +TEST_F(SecurityLoggingTest, test_nothing_enabled) +{ + eprosima::fastrtps::rtps::PropertyPolicy policy; + EXPECT_TRUE(apply_security_logging_configuration(policy)); + EXPECT_FALSE(rmw_error_is_set()); + + ASSERT_TRUE(policy.properties().empty()); +} + +TEST_F(SecurityLoggingTest, test_log_to_file) +{ + custom_setenv(log_file_variable_name, "/test.log"); + + eprosima::fastrtps::rtps::PropertyPolicy policy; + EXPECT_TRUE(apply_security_logging_configuration(policy)); + EXPECT_FALSE(rmw_error_is_set()); + + ASSERT_EQ(policy.properties().size(), 2u); + + auto property = logging_plugin_property(policy.properties()); + EXPECT_EQ(property.name(), logging_plugin_property_name); + EXPECT_EQ(property.value(), "builtin.DDS_LogTopic"); + + property = log_file_property(policy.properties()); + EXPECT_EQ(property.name(), log_file_property_name); + EXPECT_EQ(property.value(), "/test.log"); +} + +TEST_F(SecurityLoggingTest, test_log_publish_true) +{ + custom_setenv(log_publish_variable_name, "true"); + + eprosima::fastrtps::rtps::PropertyPolicy policy; + EXPECT_TRUE(apply_security_logging_configuration(policy)); + EXPECT_FALSE(rmw_error_is_set()); + + ASSERT_EQ(policy.properties().size(), 2u); + + auto property = logging_plugin_property(policy.properties()); + EXPECT_EQ(property.name(), logging_plugin_property_name); + EXPECT_EQ(property.value(), "builtin.DDS_LogTopic"); + + property = distribute_enable_property(policy.properties()); + EXPECT_EQ(property.name(), distribute_enable_property_name); + EXPECT_EQ(property.value(), "true"); +} + +TEST_F(SecurityLoggingTest, test_log_publish_false) +{ + custom_setenv(log_publish_variable_name, "false"); + + eprosima::fastrtps::rtps::PropertyPolicy policy; + EXPECT_TRUE(apply_security_logging_configuration(policy)); + EXPECT_FALSE(rmw_error_is_set()); + + ASSERT_EQ(policy.properties().size(), 2u); + + auto property = logging_plugin_property(policy.properties()); + EXPECT_EQ(property.name(), logging_plugin_property_name); + EXPECT_EQ(property.value(), "builtin.DDS_LogTopic"); + + property = distribute_enable_property(policy.properties()); + EXPECT_EQ(property.name(), distribute_enable_property_name); + EXPECT_EQ(property.value(), "false"); +} + +TEST_F(SecurityLoggingTest, test_log_publish_invalid) +{ + custom_setenv(log_publish_variable_name, "invalid"); + + eprosima::fastrtps::rtps::PropertyPolicy policy; + EXPECT_FALSE(apply_security_logging_configuration(policy)); + EXPECT_TRUE(rmw_error_is_set()); + EXPECT_THAT( + rmw_get_error_string().str, HasSubstr( + "ROS_SECURITY_LOG_PUBLISH is not valid: 'invalid' is not a supported value (use 'true' or " + "'false')")); +} + +TEST_F(SecurityLoggingTest, test_log_verbosity) +{ + custom_setenv(log_verbosity_variable_name, "FATAL"); + + eprosima::fastrtps::rtps::PropertyPolicy policy; + EXPECT_TRUE(apply_security_logging_configuration(policy)); + EXPECT_FALSE(rmw_error_is_set()); + + ASSERT_EQ(policy.properties().size(), 2u); + + auto property = logging_plugin_property(policy.properties()); + EXPECT_EQ(property.name(), logging_plugin_property_name); + EXPECT_EQ(property.value(), "builtin.DDS_LogTopic"); + + property = verbosity_property(policy.properties()); + EXPECT_EQ(property.name(), verbosity_property_name); + EXPECT_EQ(property.value(), "EMERGENCY_LEVEL"); +} + +TEST_F(SecurityLoggingTest, test_log_verbosity_invalid) +{ + custom_setenv(log_verbosity_variable_name, "INVALID_VERBOSITY"); + + eprosima::fastrtps::rtps::PropertyPolicy policy; + EXPECT_FALSE(apply_security_logging_configuration(policy)); + EXPECT_TRUE(rmw_error_is_set()); + EXPECT_THAT( + rmw_get_error_string().str, HasSubstr( + "ROS_SECURITY_LOG_VERBOSITY is not valid: INVALID_VERBOSITY is not a supported verbosity " + "(use FATAL, ERROR, WARN, INFO, or DEBUG)")); + + ASSERT_TRUE(policy.properties().empty()); +} + +TEST_F(SecurityLoggingTest, test_all) +{ + custom_setenv(log_file_variable_name, "/test.log"); + custom_setenv(log_publish_variable_name, "true"); + custom_setenv(log_verbosity_variable_name, "ERROR"); + + eprosima::fastrtps::rtps::PropertyPolicy policy; + EXPECT_TRUE(apply_security_logging_configuration(policy)); + EXPECT_FALSE(rmw_error_is_set()); + + ASSERT_EQ(policy.properties().size(), 4u); + + auto property = log_file_property(policy.properties()); + EXPECT_EQ(property.name(), log_file_property_name); + EXPECT_EQ(property.value(), "/test.log"); + + property = distribute_enable_property(policy.properties()); + EXPECT_EQ(property.name(), distribute_enable_property_name); + EXPECT_EQ(property.value(), "true"); + + property = verbosity_property(policy.properties()); + EXPECT_EQ(property.name(), verbosity_property_name); + EXPECT_EQ(property.value(), "ERROR_LEVEL"); +} + +#else + +TEST_F(SecurityLoggingTest, test_apply_logging_fails) +{ + custom_setenv(log_file_variable_name, "/test.log"); + + eprosima::fastrtps::rtps::PropertyPolicy policy; + EXPECT_FALSE(apply_security_logging_configuration(policy)); + EXPECT_TRUE(rmw_error_is_set()); + EXPECT_THAT(rmw_get_error_string().str, HasSubstr("Please compile Fast DDS")); +} + +#endif