diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..7921303 --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,576 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rcl +^^^^^^^^^^^^^^^^^^^^^^^^^ + +5.3.0 (2022-04-05) +------------------ +* add content-filtered-topic interfaces (`#894 `_) +* Contributors: Chen Lihui + +5.2.1 (2022-03-31) +------------------ +* Add additional null check for timer argument (`#973 `_) +* Contributors: NoyZuberi + +5.2.0 (2022-03-24) +------------------ +* Allow forward slashes within a parameter name rule in argument parsing (`#860 `_) +* Suppress false positive from clang-tidy (`#951 `_) +* Fix missing terminating \0 in rcl_context_impl_t.argv (`#969 `_) +* test_publisher_wait_all_ack depends on rcpputils (`#968 `_) +* Micro-optimizations in rcl (`#965 `_) +* If timer canceled, rcl_timer_get_time_until_next_call returns TIMER_CANCELED (`#963 `_) +* Contributors: Chris Lalancette, Haowei Wen, Ivan Santiago Paunovic, Shane Loretz, William Woodall, mauropasse + +5.1.0 (2022-03-01) +------------------ +* Add Events Executor (`#839 `_) +* Remove fastrtps customization on test_events (`#960 `_) +* Add client/service QoS getters (`#941 `_) +* introduce ROS_DISABLE_LOAN_MSG to disable can_loan_messages. (`#949 `_) +* Install includes it include/${PROJECT_NAME} (`#959 `_) +* Contributors: Miguel Company, Shane Loretz, Tomoya Fujita, iRobot ROS, mauropasse + +5.0.1 (2022-01-14) +------------------ + +5.0.0 (2021-12-14) +------------------ +* Make rcl_difference_times args const (`#955 `_) +* Update inject_on_return test skipping logic (`#953 `_) +* Fix jump callbacks being called when zero time jump thresholds used (`#948 `_) +* Only change the default logger level if default_logger_level is set (`#943 `_) +* Add Library for wait_for_entity_helpers to deduplicate compilation (`#942 `_) +* Increase Windows timeout 15 -> 25 ms (`#940 `_) +* test should check specified number of entities. (`#935 `_) +* Contributors: Jafar Abdi, Scott K Logan, Shane Loretz, Tomoya Fujita + +4.0.0 (2021-09-16) +------------------ +* Fix up documentation build for rcl when using rosdoc2 (`#932 `_) +* Include rmw_event_t instead of forward declaring it (`#933 `_) +* Contributors: Michel Hidalgo + +3.2.0 (2021-09-02) +------------------ +* Add rcl_publisher_wait_for_all_acked support. (`#913 `_) +* Add tracing instrumentation for rcl_take. (`#930 `_) +* Fix #include in C++ typesupport example in rcl_subscription_init docblock. (`#927 `_) +* Update includes after rcutils/get_env.h deprecation. (`#917 `_) +* Use proper rcl_logging return value type and compare to constant. (`#916 `_) +* Contributors: Barry Xu, Christophe Bedard + +3.1.2 (2021-04-26) +------------------ +* Fix up test_network_flow_endpoints. (`#912 `_) +* Contributors: Chris Lalancette + +3.1.1 (2021-04-12) +------------------ +* Make test_two_timers_ready_before_timeout less flaky (`#911 `_) +* Add publishing instrumentation (`#905 `_) +* Contributors: Christophe Bedard, Ivan Santiago Paunovic + +3.1.0 (2021-04-06) +------------------ +* Unique network flows (`#880 `_) +* updating quality declaration links (re: `ros2/docs.ros2.org#52 `_) (`#909 `_) +* Add functions for waiting for publishers and subscribers (`#907 `_) +* Revert "Mark cyclonedds test_service test as flakey (`#648 `_)" (`#904 `_) +* Guard against returning NULL or empty node names (`#570 `_) +* Contributors: Ananya Muddukrishna, Jacob Perron, Michel Hidalgo, shonigmann + +3.0.1 (2021-03-25) +------------------ +* Remove exceptions for rmw_connext_cpp tests. (`#903 `_) +* Contributors: Chris Lalancette + +3.0.0 (2021-03-23) +------------------ + +2.6.0 (2021-03-18) +------------------ +* Add support for rmw_connextdds (`#895 `_) +* Put an argument list of 'void' where no arguments are expected. (`#899 `_) +* Cleanup documentation for doxygen. (`#896 `_) +* Contributors: Andrea Sorbini, Chris Lalancette + +2.5.2 (2021-02-05) +------------------ +* Reference test resources directly from source tree (`#554 `_) +* Contributors: Scott K Logan + +2.5.1 (2021-01-25) +------------------ +* Re-add "Improve trigger test for graph guard condition (`#811 `_)" (`#884 `_) +* Revert "Improve trigger test for graph guard condition (`#811 `_)" (`#883 `_) +* Move the guard condition cleanup after removing callback. (`#877 `_) +* Make test_subscription_nominal_string_sequence more reliable (`#881 `_) +* Improve trigger test for graph guard condition (`#811 `_) +* Add NULL check in remap.c (`#879 `_) +* Contributors: Barry Xu, Chris Lalancette, Ivan Santiago Paunovic, Nikolai Morin + +2.5.0 (2020-12-08) +------------------ +* Add const to constant rcl_context functions (`#872 `_) +* Fix another failing test on CentOS 7 (`#863 `_) +* Update QDs to QL 1 (`#866 `_) +* Address clang static analysis issues (`#865 `_) +* Fix flaky test_info_by_topic (`#859 `_) +* Update QL (`#858 `_) +* Refactor for removing unnecessary source code (`#857 `_) +* Clarify storing of current_time (`#850 `_) +* Make tests in test_graph.cpp more reliable (`#854 `_) +* Fix for external log segfault after SIGINT (`#844 `_) +* Update tracetools QL and add to rcl_lifecycle's QD (`#845 `_) +* Make test logging rosout more reliable (`#846 `_) +* Return OK when finalizing zero-initialized contexts (`#842 `_) +* Zero initialize events an size_of_events members of rcl_wait_set_t (`#841 `_) +* Update deprecated gtest macros (`#818 `_) +* Contributors: Alejandro Hernández Cordero, Audrow Nash, Chen Lihui, Chris Lalancette, Christophe Bedard, Ivan Santiago Paunovic, Jacob Perron, Stephen Brawner, Thijs Raymakers, tomoya + +2.4.0 (2020-10-19) +------------------ +* Make sure to check the return value of rcl APIs. (`#838 `_) +* Add convenient node method to get a final topic/service name (`#835 `_) +* Contributors: Chris Lalancette, Ivan Santiago Paunovic + +2.3.0 (2020-10-19) +------------------ +* Remove redundant error formatting (`#834 `_) +* Fix memory leak in rcl_subscription_init()/rcl_publisher_init() (`#794 `_) +* Update maintainers (`#825 `_) +* Add a semicolon to RCUTILS_LOGGING_AUTOINIT. (`#816 `_) +* Improve error messages in rcl_lifecycle (`#742 `_) +* Fix memory leak on serialized message in test_publisher/subscription.cpp (`#801 `_) +* Fix memory leak because of mock test (`#800 `_) +* Spelling correction (`#798 `_) +* Fix that not to deallocate event impl in some failure case (`#790 `_) +* calling fini functions to avoid memory leak (`#791 `_) +* Contributors: Barry Xu, Chen Lihui, Chris Lalancette, Geoffrey Biggs, Ivan Santiago Paunovic, Jacob Perron, Lei Liu + +2.2.0 (2020-09-02) +------------------ +* Bump rcl arguments' API test coverage (`#777 `_) +* Fix rcl arguments' API memory leaks and bugs (`#778 `_) +* Add coverage tests wait module (`#769 `_) +* Fix wait set allocation cleanup (`#770 `_) +* Improve test coverage in rcl (`#764 `_) +* Check if rcutils_strdup() outcome immediately (`#768 `_) +* Cleanup rcl_get_secure_root() implementation (`#762 `_) +* Add fault injection macros to rcl functions (`#727 `_) +* Yield rcl_context_fini() error codes (`#763 `_) +* Do not invalidate context before successful shutdown (`#761 `_) +* Zero initialize guard condition on failed init (`#760 `_) +* Adding tests to arguments API (`#752 `_) +* Extend rcl_expand_topic_name() API test coverage (`#758 `_) +* Add coverage tests 94% to service API (`#756 `_) +* Clean up rcl_expand_topic_name() implementation (`#757 `_) +* Complete rcl enclave validation API coverage (`#751 `_) +* Cope with base function restrictions in mocks (`#753 `_) +* Fix allocation when copying arguments (`#748 `_) +* Complete rcl package's logging API test coverage (`#747 `_) +* Improve coverage to 95% in domain id, init option, rmw implementation id and log level modules (`#744 `_) +* Fix rcl package's logging API error code documentation and handling (`#746 `_) +* Fix bug error handling in get_param_files (`#743 `_) +* Complete subscription API test coverage (`#734 `_) +* increase timeouts in test_services fixtures for Connext (`#745 `_) +* Tweaks to client.c and subscription.c for cleaner init/fini (`#728 `_) +* Improve error checking and handling in subscription APIs (`#739 `_) +* Add deallocate calls to free strdup allocated memory (`#737 `_) +* Add missing calls to rcl_convert_rmw_ret_to_rcl_ret (`#738 `_) +* Add mock tests, publisher 95% coverage (`#732 `_) +* Restore env variables set in the test_failing_configuration. (`#733 `_) +* Expose qos setting for /rosout (`#722 `_) +* Reformat rmw_impl_id_check to call a testable function (`#725 `_) +* Add extra check for invalid event implementation (`#726 `_) +* Consolidate macro duplication (`#653 `_) +* Contributors: Ada-King, Dan Rose, Dirk Thomas, Jorge Perez, Michel Hidalgo, brawner, tomoya + +2.1.0 (2020-07-22) +------------------ +* Add test for subscription message lost event (`#705 `_) +* Add function rcl_event_is_valid (`#720 `_) +* Move actual domain id from node to context (`#718 `_) +* Removed doxygen warnings (`#712 `_) +* Remove some dead code. +* Make sure to call rcl_arguments_fini at the end of the test. +* Add remap needed null check (`#711 `_) +* Make public init/fini rosout publisher (`#704 `_) +* Move rcl_remap_copy to public header (`#709 `_) +* Contributors: Alejandro Hernández Cordero, Chris Lalancette, Ivan Santiago Paunovic, Jorge Perez + +2.0.0 (2020-07-09) +------------------ +* Implement a generic way to change logging levels (`#664 `_) +* Remove domain_id and localhost_only from node_options (`#708 `_) +* Add coverage tests (`#703 `_) +* Add bad arguments tests for coverage (`#698 `_) +* Remove unused internal prototypes (`#699 `_) +* Update quality declaration and coverage (`#674 `_) +* Add setter and getter for domain_id in rcl_init_options_t (`#678 `_) +* Remove unused pytest dependency from rcl. (`#695 `_) +* Fix link to latest API docs (`#692 `_) +* Keep domain id if ROS_DOMAIN_ID is invalid. (`#689 `_) +* Remove unused check context.c (`#691 `_) +* Add check rcl_node_options_copy invalid out (`#671 `_) +* Update tracetools' QL to 2 in rcl's QD (`#690 `_) +* Improve subscription coverage (`#681 `_) +* Improve rcl timer test coverage (`#680 `_) +* Improve wait sets test coverage (`#683 `_) +* Contributors: Alejandro Hernández Cordero, Chen Lihui, Chris Lalancette, Christophe Bedard, Ivan Santiago Paunovic, Jorge Perez, Michel Hidalgo, tomoya + +1.2.0 (2020-06-18) +------------------ +* Improve rcl init test coverage. (`#684 `_) +* Improve clock test coverage. (`#685 `_) +* Add message lost event (`#673 `_) +* Minor fixes to rcl clock implementation. (`#688 `_) +* Improve enclave validation test coverage. (`#682 `_) +* Use RCL_RET\_* codes only. (`#686 `_) +* Fixed doxygen warnings (`#677 `_) +* Add tests for rcl package (`#668 `_) +* Remove logging_external_interface.h, provided by rcl_logging_interface package now (`#676 `_) +* Print RCL_LOCALHOST_ENV_VAR if error happens via rcutils_get_env. (`#672 `_) +* Contributors: Alejandro Hernández Cordero, Chris Lalancette, Ivan Santiago Paunovic, Jorge Perez, Michel Hidalgo, tomoya + +1.1.5 (2020-06-03) +------------------ +* Fix conversions between rmw_localhost_only_t and bool (`#670 `_) +* Contributors: Jorge Perez + +1.1.4 (2020-06-02) +------------------ +* Ensure rcl_publisher_init() fails safely (`#667 `_) +* Contributors: Michel Hidalgo + +1.1.3 (2020-06-01) +------------------ +* Add Security Vulnerability Policy pointing to REP-2006 (`#661 `_) +* Add tests to publisher and init modules of rcl (`#657 `_) +* Contributors: Chris Lalancette, Jorge Perez + +1.1.2 (2020-05-28) +------------------ +* Improve docblocks (`#659 `_) +* Contributors: Alejandro Hernández Cordero + +1.1.1 (2020-05-26) +------------------ + +1.1.0 (2020-05-22) +------------------ +* Expose rcl default logging output handler (`#660 `_) +* Remove deprecated functions (`#658 `_) +* Warn about unused return value for set_logger_level (`#652 `_) +* Mark cyclonedds test_service test as flakey (`#648 `_) +* Convert sleep_for into appropriate logic in tests(`#631 `_) +* Reduce timeouts in tests(`#613 `_) +* Add tests for time.c and timer.c (`#599 `_) +* Update Quality Declaration for 1.0 (`#647 `_) +* Contributors: Barry Xu, Dirk Thomas, Ivan Santiago Paunovic, Jorge Perez, Tully Foote, brawner + +1.0.0 (2020-05-12) +------------------ +* Remove MANUAL_BY_NODE liveliness API (`#645 `_) +* Make test_two_timers* more reliable (`#640 `_) +* Contributors: Ivan Santiago Paunovic + +0.9.1 (2020-05-08) +------------------ +* Included features (`#644 `_) +* Current state Quality Declaration (`#639 `_) +* Initialize service timestamps to 0 and test. (`#642 `_) +* Contributors: Alejandro Hernández Cordero, Ingo Lütkebohle, Jorge Perez + +0.9.0 (2020-04-29) +------------------ +* Fix std::string construction in test (`#636 `_) +* Add basic functionality tests for validate_enclave_name and subscription (`#624 `_) +* Save allocator for RCL_CLOCK_UNINITIALIZED clock (`#623 `_) +* Implement service info structure with timestamps (`#627 `_) +* Add support for taking a sequence of messages (`#614 `_) +* Message info with timestamps support in rcl (`#619 `_) +* Don't call ``rcl_logging_configure/rcl_logging_fini`` in ``rcl_init/rcl_shutdown`` (`#579 `_) +* Export targets in a addition to include directories / libraries (`#629 `_) +* Document rcl_pub/etc_fini() must come before rcl_node_fini() (`#625 `_) +* Update security environment variables (`#617 `_) +* Add visibility to rcl_timer_get_allocator (`#610 `_) +* Fix test_publisher memory leaks reported by asan (`#567 `_) +* security-context -> enclave (`#612 `_) +* Rename rosidl_generator_c namespace to rosidl_runtime_c (`#616 `_) +* Rename rosidl_generator_cpp namespace to rosidl_runtime_cpp (`#615 `_) +* Fix security directory lookup for '/' security contexts (`#609 `_) +* Changed rosidl_generator_c/cpp to rosidl_runtime_c/cpp (`#588 `_) +* Remove deprecated CLI rules (`#603 `_) +* Use keystore root as security root directory, and not contexts folder (`#607 `_) +* Remove tinydir_vendor dependency (`#608 `_) +* Add missing allocator check for NULL (`#606 `_) +* Change naming style for private functions (`#597 `_) +* Switch to one Participant per Context (`#515 `_) +* Support for ON_REQUESTED_INCOMPATIBLE_QOS and ON_OFFERED_INCOMPATIBLE_QOS events (`#535 `_) +* Small typo fix (`#604 `_) +* Update docstring with new possible return code (`#600 `_) +* Add missing node destruction (`#601 `_) +* Test that nodes are returned with correct multiplicity (`#598 `_) +* Trigger guard condition when timer is reset (`#589 `_) +* Clock API improvements (`#580 `_) +* Fix memory leak in rcl_arguments (`#564 `_) +* Don't check history depth if RMW_QOS_POLICY_HISTORY_KEEP_ALL (`#593 `_) +* Fix alloc-dealloc-mismatch(new->free) in test_info_by_topic (`#469 `_) (`#569 `_) +* Use 10sec lifespan in rosout publisher qos (`#587 `_) +* Document clock types (`#578 `_) +* Make rosout publisher transient local with a depth of 1000 (`#582 `_) +* Enable TestInfoByTopicFixture unit tests for other rmw_implementations (`#583 `_) +* Fix memory leak in test_subscription_nominal (`#469 `_) (`#562 `_) +* Update rmw_topic_endpoint_info_array usage (`#576 `_) +* Add rcl versions of rmw_topic_endpoint_info* types (`#558 `_) +* Enable test for rcl_get_subscriptions_info_by_topic / rcl_get_publishers_info_by_topic for Cyclone (`#572 `_) +* Fixed missing initialization and fixed qos checking in test (`#571 `_) +* Fix test_count_matched memory leaks reported by asan `#567 `_ (`#568 `_) +* Code style only: wrap after open parenthesis if not in one line (`#565 `_) +* Fix return type of rcl_publisher_get_subscription_count() (`#559 `_) +* Fix doc strings (`#557 `_) +* Implement functions to get publisher and subcription informations like QoS policies from topic name (`#511 `_) +* Use absolute topic name for ``rosout`` (`#549 `_) +* Set allocator before goto fail (`#546 `_) +* Add public facing API for validating rcl_wait_set_t (`#538 `_) +* Add flag to enable/disable rosout logging in each node individually. (`#532 `_) +* Treat __name the same as __node (`#494 `_) +* Contributors: Alejandro Hernández Cordero, Barry Xu, Chris Lalancette, Dan Rose, Dennis Potman, Dirk Thomas, DongheeYe, Ingo Lütkebohle, Ivan Santiago Paunovic, Jacob Perron, Jaison Titus, Jorge Perez, Miaofei Mei, Michael Carroll, Michel Hidalgo, Mikael Arguedas, P. J. Reed, Ruffin, Shane Loretz, William Woodall, y-okumura-isp + +0.8.3 (2019-11-08) +------------------ +* Support CLI parameter overrides using dots instead of slashes. (`#530 `_) + Signed-off-by: Michel Hidalgo +* Contributors: Michel Hidalgo + +0.8.2 (2019-10-23) +------------------ +* Remove the prototype from rcl_impl_getenv. (`#525 `_) +* Use return_loaned_message_from (`#523 `_) +* Avoid ready_fn and self.proc_info (`#522 `_) +* Add localhost option to node creation (`#520 `_) +* Add initial instrumentation (`#473 `_) +* Zero copy api (`#506 `_) +* Don't create rosout publisher instance unless required. (`#514 `_) +* Handle zero non-ROS specific args properly in rcl_remove_ros_arguments (`#518 `_) +* Update rcl_node_init docstring (`#517 `_) +* Remove vestigial references to rcl_ok() (`#516 `_) +* Add mechanism to pass rmw impl specific payloads during pub/sub creation (`#513 `_) +* Contributors: Brian Marchi, Chris Lalancette, Ingo Lütkebohle, Jacob Perron, Karsten Knese, Michel Hidalgo, Peter Baughman, William Woodall, tomoya + +0.8.1 (2019-10-08) +------------------ +* Switch the default logging implementation to spdlog. +* Contributors: Chris Lalancette + +0.8.0 (2019-09-26) +------------------ +* Delete rcl_impl_getenv, replaced by rcutils_get_env (`#502 `_) +* Parse CLI parameters and YAML files (`#508 `_) +* Add specific return code for non existent node (`#492 `_) +* Add node name and namespace validation to graph functions (`#499 `_) +* Bring back deprecated CLI arguments (`#496 `_) +* Polish rcl arguments implementation (`#497 `_) +* Uncoment some test_graph test cases after fix in rmw_fastrtps (`ros2/rmw_fastrtps#316 `_) (`#498 `_) +* Promote special CLI rules to flags (`#495 `_) +* Fail fast on invalid ROS arguments (`#493 `_) +* Enforce -r/--remap flags. (`#491 `_) +* Support parameter overrides and remap rules flags on command line (`#483 `_) +* Allow get_node_names to return result in any order (`#488 `_) +* rosout init and fini marked as RCL_PUBLIC (`#479 `_) +* included header in logging_rosout.c (`#478 `_) +* Migrate to '--ros-args ... [--]'-based ROS args extraction (`#477 `_) +* Improve security error messages (`#480 `_) +* Add function for getting clients by node (`#459 `_) +* Remove special case check for manual_by_node for rmw_fastrtps (`#467 `_) +* Fix memory leak of 56 bytes in test_graph +* Change tests to try MANUAL_BY_TOPIC liveliness for FastRTPS (`#465 `_) +* Implement get_actual_qos() for subscriptions (`#455 `_) +* Log warning when remapping to an invalid node name (`#454 `_) +* Use size_t printf format for size_t variable (`#453 `_) +* Contributors: Alberto Soragna, Emerson Knapp, Jacob Perron, M. M, Michel Hidalgo, Mikael Arguedas, Víctor Mayoral Vilches, eboasson, ivanpauno + +0.7.4 (2019-05-29) +------------------ +* Fix tests now that FastRTPS correctly reports that liveliness is not supported (`#452 `_) +* In test_events, wait for discovery to be complete bidirectionally before moving on (`#451 `_) +* fix leak in test_service (`#447 `_) +* fix leak in test_guard_condition (`#446 `_) +* fix leak in test_get_actual_qos (`#445 `_) +* fix leak in test_expand_topic_name (`#444 `_) +* Contributors: Abby Xu, Emerson Knapp + +0.7.3 (2019-05-20) +------------------ +* Fixed memory leak in ``test_client`` (`#443 `_) +* Fixed memory leaks in ``test_wait.cpp`` (`#439 `_) +* Fixed memory leak in ``test_context`` (`#441 `_) +* Fixed memory leak in ``test_init`` (`#440 `_) +* Enabled rcl ``test_events`` unit tests on macOS (`#433 `_) +* Enabled deadline tests for FastRTPS (`#438 `_) +* Corrected use of ``launch_testing.assert.assertExitCodes`` (`#437 `_) +* Reverted "Changes the default 3rd party logger from rcl_logging_noop to… (`#436 `_) +* Fixed memory leaks in ``test_security_directory`` (`#420 `_) +* Fixed a memory leak in rcl context fini (`#434 `_) +* Contributors: Abby Xu, Cameron Evans, Chris Lalancette, Dirk Thomas, M. M, ivanpauno + +0.7.2 (2019-05-08) +------------------ +* Changes the default 3rd party logger from rcl_logging_noop to rcl_logging_log4cxx (`#425 `_) +* fix leak in node.c (`#424 `_) +* Add new RCL_RET_UNSUPPORTED (`#432 `_) +* New interfaces and their implementations for QoS features (`#408 `_) +* Add an allocator to the external logging initialization. (`#430 `_) +* fix buffer overflow in test_security_dir (`#423 `_) +* Rmw preallocate (`#428 `_) +* Use new test interface definitions (`#427 `_) +* Migrate launch tests to new launch_testing features & API (`#405 `_) +* Fix argument passed to logging macros (`#421 `_) +* Make sure to initialize the bool field. (`#426 `_) +* Contributors: Abby Xu, Chris Lalancette, Emerson Knapp, Jacob Perron, M. M, Michael Carroll, Michel Hidalgo, Nick Burek, Thomas Moulard + +0.7.1 (2019-04-29) +------------------ +* Replaced reinterperet_cast with static_cast. (`#410 `_) +* Fixed leak in __wait_set_clean_up. (`#418 `_) +* Updated initialization of rmw_qos_profile_t struct instances. (`#416 `_) +* Contributors: Dirk Thomas, M. M, jhdcs + +0.7.0 (2019-04-14) +------------------ +* Added more test cases for graph API + fix bug. (`#404 `_) +* Fixed missing include. (`#413 `_) +* Updated to use pedantic. (`#412 `_) +* Added function to get publisher actual qos settings. (`#406 `_) +* Refactored graph API docs. (`#401 `_) +* Updated to use ament_target_dependencies where possible. (`#400 `_) +* Fixed regression around fully qualified node name. (`#402 `_) +* Added function rcl_names_and_types_init. (`#403 `_) +* Fixed uninitialize sequence number of client. (`#395 `_) +* Added launch along with launch_testing as test dependencies. (`#393 `_) +* Set symbol visibility to hidden for rcl. (`#391 `_) +* Updated to split test_token to avoid compiler note. (`#392 `_) +* Dropped legacy launch API usage. (`#387 `_) +* Improved security directory lookup. (`#332 `_) +* Enforce non-null argv values on rcl_init(). (`#388 `_) +* Removed incorrect argument documentation. (`#361 `_) +* Changed error to warning for multiple loggers. (`#384 `_) +* Added rcl_node_get_fully_qualified_name. (`#255 `_) +* Updated rcl_remap_t to use the PIMPL pattern. (`#377 `_) +* Fixed documentation typo. (`#376 `_) +* Removed test circumvention now that a bug is fixed in rmw_opensplice. (`#368 `_) +* Updated to pass context to wait set, and fini rmw context. (`#373 `_) +* Updated to publish logs to Rosout. (`#350 `_) +* Contributors: AAlon, Dirk Thomas, Jacob Perron, M. M, Michael Carroll, Michel Hidalgo, Mikael Arguedas, Nick Burek, RARvolt, Ross Desmond, Sachin Suresh Bhat, Shane Loretz, William Woodall, ivanpauno + +0.6.4 (2019-01-11) +------------------ +* Added method for accessing rmw_context from rcl_context (`#372 `_) +* Added guard against bad allocation when calling rcl_arguments_copy() (`#367 `_) +* Updated to ensure that context instance id storage is aligned correctly (`#365 `_) +* Fixed error from uncrustify v0.68 (`#364 `_) +* Contributors: Jacob Perron, William Woodall, sgvandijk + +0.6.3 (2018-12-13) +------------------ +* Set rmw_wait timeout using ros timers too (`#357 `_) +* Contributors: Shane Loretz + +0.6.2 (2018-12-13) +------------------ +* Updated docs about possibility of rcl_take not taking (`#356 `_) +* Bugfix: ensure NULL timeout is passed to rmw_wait() when min_timeout is not set + Otherwise, there is a risk of integer overflow (e.g. in rmw_fastrtps) and rmw_wait() will wake immediately. +* Contributors: Jacob Perron, William Woodall + +0.6.1 (2018-12-07) +------------------ +* Added new cli parameters for configuring the logging. (`#327 `_) +* Added node graph api to rcl. (`#333 `_) +* Fixed compiler warning in clang (`#345 `_) +* Refactored init to not be global (`#336 `_) +* Methods to retrieve matched counts on pub/sub. (`#326 `_) +* Updated to output index in container when adding an entity to a wait set. (`#335 `_) +* Contributors: Jacob Perron, Michael Carroll, Nick Burek, Ross Desmond, William Woodall + +0.6.0 (2018-11-16) +------------------ +* Updated to expand node_secure_root using local_namespace (`#300 `_) +* Moved stdatomic helper to rcutils (`#324 `_) +* Added subfolder argument to the ROSIDL_GET_SRV_TYPE_SUPPORT macro (`#322 `_) +* Updated to use new error handling API from rcutils (`#314 `_) +* Fixed minor documentation issues (`#305 `_) +* Added macro semicolons (`#303 `_) +* Added Rcl timer with ros time (`#286 `_) +* Updated to ensure that timer period is non-negative (`#295 `_) +* Fixed calculation of next timer call (`#291 `_) +* Updated to null deallocated jump callbacks (`#294 `_) +* Included namespaces in get_node_names. (`#287 `_) +* Fixed documentation issues (`#288 `_) +* Updated to check if pointers are null before calling memset (`#290 `_) +* Added multiple time jump callbacks to clock (`#284 `_) +* Consolidated wait set functions (`#285 `_) + * Consolidate functions to clear wait set + Added rcl_wait_set_clear() + Added rcl_wait_set_resize() + Removed + rcl_wait_set_clear_subscriptions() + rcl_wait_set_clear_guard_conditions() + rcl_wait_set_clear_clients() + rcl_wait_set_clear_services() + rcl_wait_set_clear_timers() + rcl_wait_set_resize_subscriptions() + rcl_wait_set_resize_guard_conditions() + rcl_wait_set_resize_timers() + rcl_wait_set_resize_clients() + rcl_wait_set_resize_services() +* ROS clock storage initially set to zero (`#283 `_) +* Fixed issue with deallocation of parameter_files (`#279 `_) +* Update to initialize memory before sending a message (`#277 `_) +* Set error message when clock type is not ROS_TIME (`#275 `_) +* Copy allocator passed in to clock init (`#274 `_) +* Update to initialize timer with clock (`#272 `_) +* Updated to use test_msgs instead of std_msgs in tests (`#270 `_) +* Added regression test for node:__ns remapping (`#263 `_) +* Updated to support Uncrustify 0.67 (`#266 `_) +* Contributors: Chris Lalancette, Chris Ye, Dirk Thomas, Jacob Perron, Michael Carroll, Mikael Arguedas, Ruffin, Shane Loretz, William Woodall, dhood + +0.5.0 (2018-06-25) +------------------ +* Updated code to only use ``rcutils_allocator_t`` and not use system memory functions directly. (`#261 `_) +* Changed code to use ``rcutils_format_string()`` rather than ``malloc`` and ``rcutils_snprintf()`` (`#240 `_) +* Added functions for dealing with serialized messages. (`#170 `_) +* Updated to use ``test_msgs`` instead of ``example_interfaces``. (`#259 `_) +* Added regression test for the Connext specific 'wrong type writer' error. (`#257 `_) +* Added the ability to set the default logger level from command line. (`#256 `_) +* Refactored the ``memory_tools`` testing API to ``osrf_testing_tools_cpp`` (`#238 `_) +* Added support for passing YAML parameter files via the command line arguments. (`#253 `_) +* Migrated existing uses of ``launch`` to use the same API in it's new API ``launch.legacy``. (`#250 `_) +* Added a printed warning if non-FQN namespace remapping is passed. (`#248 `_) +* Made some changes toward MISRA C compliance. (`#229 `_) +* Changed ``rcl_node_init()`` so that it now copies node options passed into it (`#231 `_) +* Fixed some memory leaks in ``test_arguments`` (`#230 `_) +* Extended static remapping feature with support for the url scheme (`#227 `_) +* Made a change to force ``rcl_arguments_t`` to be zero initialized. (`#225 `_) +* Updated documentation for ``rmw_get_node_names()`` to mention the potential for null values (`#214 `_) +* Fix an issue with signed time difference. (`#224 `_) +* Changed library export order to fix static linking (`#216 `_) +* Implemented static remapping over command line arguments (`#217 `_ and `#221 `_) +* Added a sized validation function for the topic name as ``rcl_validate_topic_name_with_size()`` (`#220 `_) +* Added a logger name and stored it in the rcl node structure (`#212 `_) +* Changed ``rcutils_time_point_value_t`` type from ``uint64_t`` to ``int64_t`` (`#208 `_) +* Fixed a potential bug by resetting the ``RMWCount`` when using the ``DEALLOC`` macro on rmw storage of a wait set (`#209 `_ and `#211 `_) + * Signed-off-by: jwang +* Fixed a potential bug by resetting ``wait_set`` type index in the ``SET_RESIZE`` macro (`#207 `_) + * Signed-off-by: jwang +* Removed a slash behind ``SET_CLEAR`` MACRO (`#206 `_) + * Signed-off-by: jwang +* Changed rmw result validation string to not ever return nullptr (`#193 `_) + * Signed-off-by: Ethan Gao +* Clarified that ``rcl_take_response()`` populates the ``request_header`` (`#205 `_) +* Removed a now obsolete connext workaround (`#203 `_) +* Fixed a potential segmentation fault due to a nullptr dereference (`#202 `_) + * Signed-off-by: Ethan Gao +* Contributors: Dirk Thomas, Ethan Gao, Karsten Knese, Michael Carroll, Mikael Arguedas, Shane Loretz, William Woodall, dhood, jwang11, serge-nikulin diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..11462f1 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,141 @@ +cmake_minimum_required(VERSION 3.5) + +project(rcl) + +find_package(ament_cmake_ros REQUIRED) + +find_package(rcl_interfaces REQUIRED) +find_package(rcl_logging_interface REQUIRED) +find_package(rcl_yaml_param_parser REQUIRED) +find_package(rcutils REQUIRED) +find_package(rmw REQUIRED) +find_package(rmw_implementation REQUIRED) +find_package(rosidl_runtime_c REQUIRED) +find_package(tracetools REQUIRED) + +include(cmake/rcl_set_symbol_visibility_hidden.cmake) +include(cmake/get_default_rcl_logging_implementation.cmake) +get_default_rcl_logging_implementation(RCL_LOGGING_IMPL) + +# Default to C11 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 11) +endif() + +# 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() + +set(${PROJECT_NAME}_sources + src/rcl/arguments.c + src/rcl/client.c + src/rcl/common.c + src/rcl/context.c + src/rcl/domain_id.c + src/rcl/event.c + src/rcl/expand_topic_name.c + src/rcl/graph.c + src/rcl/guard_condition.c + src/rcl/init.c + src/rcl/init_options.c + src/rcl/lexer.c + src/rcl/lexer_lookahead.c + src/rcl/localhost.c + src/rcl/logging_rosout.c + src/rcl/logging.c + src/rcl/log_level.c + src/rcl/network_flow_endpoints.c + src/rcl/node.c + src/rcl/node_options.c + src/rcl/publisher.c + src/rcl/remap.c + src/rcl/node_resolve_name.c + src/rcl/rmw_implementation_identifier_check.c + src/rcl/security.c + src/rcl/service.c + src/rcl/subscription.c + src/rcl/time.c + src/rcl/timer.c + src/rcl/validate_enclave_name.c + src/rcl/validate_topic_name.c + src/rcl/wait.c +) + +add_library(${PROJECT_NAME} ${${PROJECT_NAME}_sources}) +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$") +# specific order: dependents before dependencies +ament_target_dependencies(${PROJECT_NAME} + "rcl_interfaces" + "rcl_logging_interface" + "rcl_yaml_param_parser" + "rcutils" + "rmw" + "rmw_implementation" + ${RCL_LOGGING_IMPL} + "rosidl_runtime_c" + "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 "RCL_BUILDING_DLL") +rcl_set_symbol_visibility_hidden(${PROJECT_NAME} LANGUAGE "C") + +if(BUILD_TESTING AND NOT RCUTILS_DISABLE_FAULT_INJECTION) + target_compile_definitions(${PROJECT_NAME} PUBLIC RCUTILS_ENABLE_FAULT_INJECTION) +endif() + +install( + TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +# rcl_lib_dir is passed as APPEND_LIBRARY_DIRS for each ament_add_gtest call so +# the librcl that they link against is on the library path. +# This is especially important on Windows. +# This is overwritten each loop, but which one it points to doesn't really matter. +set(rcl_lib_dir "$") + +# Export old-style CMake variables +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +# Export modern CMake targets +ament_export_targets(${PROJECT_NAME}) + +# specific order: dependents before dependencies +ament_export_dependencies(ament_cmake) +ament_export_dependencies(rcl_interfaces) +ament_export_dependencies(rcl_logging_interface) +ament_export_dependencies(rcl_yaml_param_parser) +ament_export_dependencies(rmw_implementation) +ament_export_dependencies(rmw) +ament_export_dependencies(rcutils) +ament_export_dependencies(${RCL_LOGGING_IMPL}) +ament_export_dependencies(rosidl_runtime_c) +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 "rcl-extras.cmake") + +install( + DIRECTORY cmake + DESTINATION share/${PROJECT_NAME} +) +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) diff --git a/Doxyfile b/Doxyfile new file mode 100644 index 0000000..486ffbf --- /dev/null +++ b/Doxyfile @@ -0,0 +1,28 @@ +# All settings not listed here will use the Doxygen default values. + +PROJECT_NAME = "rcl" +PROJECT_NUMBER = master +PROJECT_BRIEF = "C API providing common ROS client library functionality." + +INPUT = ./include +RECURSIVE = YES +OUTPUT_DIRECTORY = doc_output + +SORT_MEMBER_DOCS = NO + +GENERATE_LATEX = NO + +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = YES +EXPAND_ONLY_PREDEF = YES +PREDEFINED += RCL_ALIGNAS(x)= +PREDEFINED += RCL_PUBLIC= +PREDEFINED += RCL_WARN_UNUSED= + +# 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/rmw.tag=http://docs.ros2.org/latest/api/rmw/" +TAGFILES += "../../../../doxygen_tag_files/rcutils.tag=http://docs.ros2.org/latest/api/rcutils/" +# Uncomment to generate tag files for cross-project linking. +#GENERATE_TAGFILE = "../../../../doxygen_tag_files/rcl.tag" diff --git a/QUALITY_DECLARATION.md b/QUALITY_DECLARATION.md new file mode 100644 index 0000000..cef37a7 --- /dev/null +++ b/QUALITY_DECLARATION.md @@ -0,0 +1,258 @@ +This document is a declaration of software quality for the `rcl` package, based on the guidelines in [REP-2004](https://www.ros.org/reps/rep-2004.html). + +# `rcl` Quality Declaration + +The package `rcl` claims to be in the **Quality Level 1** category when it is used with a **Quality Level 1** middleware. + +Below are the rationales, notes, and caveats for this claim, organized by each requirement listed in the [Package Quality Categories in REP-2004](https://www.ros.org/reps/rep-2004.html). + +## Version Policy [1] + +### Version Scheme [1.i] + +`rcl` 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] + +`rcl` 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`](./include/rcl) directory of the package, headers in any other folders are not installed and considered private. + +### API Stability Policy [1.iv] + +`rcl` will not break public API within a released ROS distribution, i.e. no major releases once the ROS distribution is released. + +### ABI Stability Policy [1.v] + +`rcl` contains C code and therefore must be concerned with ABI stability, and will maintain ABI stability within a ROS distribution. + +### ABI and ABI Stability Within a Released ROS Distribution [1.vi] + +`rcl` will not break API nor ABI within a released ROS distribution, i.e. no major releases once the ROS distribution is released. + +## Change Control Process [2] + +### Change Requests [2.i] + +All changes will occur through a pull request, check the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process) for additional information. + +### 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] + +All pull requests will be peer-reviewed, check [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#change-control-process) for additional information. + +### Continuous Integration [2.iv] + +All pull requests must pass CI on all [tier 1 platforms](https://www.ros.org/reps/rep-2000.html#support-tiers). + +Currently nightly results can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rcl/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rcl/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rcl/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rcl/) + +### Documentation Policy [2.v] + +All pull requests must resolve related documentation changes before merging. + +## Documentation [3] + +### Feature Documentation [3.i] + +`rcl` provides the main elements of its API listed using doxygen. Refer to the [ROS2 concepts](https://docs.ros.org/en/rolling/Concepts.html) and [ROS2 Client Libraries](https://docs.ros.org/en/rolling/Concepts/About-ROS-2-Client-Libraries.html) pages for reference of elements covered by this package. + +### Public API Documentation [3.ii] + +`rcl` has embedded API documentation and it is generated using doxygen. Currently, its latest version is hosted [here](http://docs.ros2.org/latest/api/rcl/). + +New additions to the public API require documentation before being added. + +### License [3.iii] + +The license for `rcl` 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. [Here](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rcl/) can be found a list with the latest results of the various linters being run on the package. + +### Copyright Statements [3.iv] + +The copyright holders each provide a statement of copyright in each source code file in `rcl`. + +There is an automated test which runs a linter that ensures each file has at least one copyright statement. Latest linter result report can be seen [Here](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rcl/copyright/). + +## Testing [4] + +### Feature Testing [4.i] + +Most features in `rcl` have corresponding tests which simulate typical usage, and they are located in the [`test`](./test) directory. +New features are required to have tests before being added. +Currently nightly test results can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rcl/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rcl/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rcl/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rcl/) + +### Public API Testing [4.ii] + +Each part of the public API has tests, and new additions or changes to the public API require tests before being added. The tests aim to cover both typical usage and corner cases, but are quantified by contributing to code coverage. + +The following functions are partially supported: `rcl_take_loaned_message`, `rcl_return_loaned_message_from_subscription`, `rcl_borrow_loaned_message`, `rcl_return_loaned_message_from_publisher` and `rcl_publish_loaned_message` because they are not currently supported on Tier 1 RMW providers. + +### Coverage [4.iii] + +`rcl` 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 line coverage instead of branch coverage. + +This includes: + +- tracking and reporting line coverage statistics +- 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/nightly_linux_coverage/lastSuccessfulBuild/cobertura/src_ros2_rcl_rcl_src_rcl/). A description of how coverage statistics are calculated is summarized in this page ["ROS 2 Onboarding Guide"](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#note-on-coverage-runs). + +### Performance [4.iv] + +`rcl` follows the recommendations for performance testing of C code in the [ROS 2 Developer Guide](https://docs.ros.org/en/rolling/Contributing/Developer-Guide.html#performance), and opts to do performance analysis on each release rather than each change. + +System level performance benchmarks that cover features of `rcl` can be found at: +* [Benchmarks](http://build.ros2.org/view/Rci/job/Rci__benchmark_ubuntu_focal_amd64/BenchmarkTable/) +* [Performance](http://build.ros2.org/view/Rci/job/Rci__nightly-performance_ubuntu_focal_amd64/lastCompletedBuild/) + +Changes that introduce regressions in performance must be adequately justified in order to be accepted and merged. + +### Linters and Static Analysis [4.v] + +`rcl` uses and passes all the ROS2 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). Passing implies there are no linter/static errors when testing against CI of supported platforms. + +Currently nightly test results can be seen here: +* [linux-aarch64_release](https://ci.ros2.org/view/nightly/job/nightly_linux-aarch64_release/lastBuild/testReport/rcl/) +* [linux_release](https://ci.ros2.org/view/nightly/job/nightly_linux_release/lastBuild/testReport/rcl/) +* [mac_osx_release](https://ci.ros2.org/view/nightly/job/nightly_osx_release/lastBuild/testReport/rcl/) +* [windows_release](https://ci.ros2.org/view/nightly/job/nightly_win_rel/lastBuild/testReport/rcl/) + +## Dependencies [5] + +Below are evaluations of each of `rcl`'s run-time and build-time dependencies that have been determined to influence the quality. + +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 ROS Dependencies [5.i] + +#### `rmw` + +The `rmw` package provides the API used by `rcl` to interact with the underlying middleware in an abstract way. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw/blob/master/rmw/QUALITY_DECLARATION.md). + +#### `rcl_interfaces` + +The `rcl_interfaces` package provides some common ROS Message and ROS Service types which are used to implement certain client library features. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_interfaces/blob/master/rcl_interfaces/QUALITY_DECLARATION.md). + +#### `rcl_logging_spdlog` + +The `rcl_logging_spdlog` package provides the default logging implementation by wrapping the `spdlog` library. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl_logging/blob/master/rcl_logging_spdlog/QUALITY_DECLARATION.md). + +#### `rcl_yaml_param_parser` + +The `rcl_yaml_param_parser` package provides an API that is used to parse YAML configuration files which may be used to configure ROS and specific nodes. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcl/blob/master/rcl_yaml_param_parser/QUALITY_DECLARATION.md). + +#### `rcutils` + +The `rcutils` package provides an API which contains common utilities and data structures needed when programming in C. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rcutils/blob/master/QUALITY_DECLARATION.md). + +#### `rmw_implementation` + +The `rmw_implementation` package provides access to the default rmw implementation, and provides the ability to dynamically switch rmw implementations if more than one is available. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rmw_implementation/blob/master/rmw_implementation/QUALITY_DECLARATION.md). + +#### `rosidl_runtime_c` + +The `rosidl_runtime_c` package provides runtime interfaces in C based on user defined ROS Messages and ROS Services, as well as associated support functions for those types. + +It is **Quality Level 1**, see its [Quality Declaration document](https://github.com/ros2/rosidl/blob/master/rosidl_runtime_c/QUALITY_DECLARATION.md). + +#### `tracetools` + +The `tracetools` package provides utilities for instrumenting the code in `rcl` so that it may be traced for debugging and performance analysis. + +It is **Quality Level 1**, see its [Quality Declaration document](https://gitlab.com/ros-tracing/ros2_tracing/-/blob/master/tracetools/QUALITY_DECLARATION.md). + +### Optional Direct Runtime ROS Dependencies [5.ii] + +`rcl` has no optional Direct Runtime ROS dependencies that need to be considered for this declaration. + +### Direct Runtime non-ROS Dependency [5.iii] + +`rcl` has no Direct Runtime non-ROS dependencies that need to be considered for this declaration. + +## Platform Support [6] + +`rcl` 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. + +## 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 `rcl` 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..75eb948 --- /dev/null +++ b/README.md @@ -0,0 +1,7 @@ +## rcl + +Library to support implementation of language specific ROS Client Libraries. + +Features are described in detail at [http://docs.ros2.org](http://docs.ros2.org/latest/api/rcl/index.html) + +This package claims to be in the **Quality Level 1** category, see the [Quality Declaration](./QUALITY_DECLARATION.md) for more details. diff --git a/cmake/get_default_rcl_logging_implementation.cmake b/cmake/get_default_rcl_logging_implementation.cmake new file mode 100644 index 0000000..ebd8a0c --- /dev/null +++ b/cmake/get_default_rcl_logging_implementation.cmake @@ -0,0 +1,46 @@ +# Copyright 2018 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# Get the package name of the default logging implementation. +# +# Either selecting it using the variable RCL_LOGGING_IMPLEMENTATION or +# choosing a default from the available implementations. +# +# :param var: the output variable name containing the package name +# :type var: string +# +macro(get_default_rcl_logging_implementation var) + + # if logging implementation already specified or RCL_LOGGING_IMPLEMENTATION environment variable + # is set then use that, otherwise default to using rcl_logging_noop + if(NOT "${RCL_LOGGING_IMPLEMENTATION}" STREQUAL "") + set(_logging_implementation "${RCL_LOGGING_IMPLEMENTATION}") + elseif(NOT "$ENV{RCL_LOGGING_IMPLEMENTATION}" STREQUAL "") + set(_logging_implementation "$ENV{RCL_LOGGING_IMPLEMENTATION}") + else() + set(_logging_implementation rcl_logging_spdlog) + endif() + + # persist implementation decision in cache + # if it was not determined dynamically + set( + RCL_LOGGING_IMPLEMENTATION "${_logging_implementation}" + CACHE STRING "select rcl logging implementation to use" FORCE + ) + + find_package("${_logging_implementation}" REQUIRED) + + set(${var} ${_logging_implementation}) +endmacro() diff --git a/cmake/rcl_set_symbol_visibility_hidden.cmake b/cmake/rcl_set_symbol_visibility_hidden.cmake new file mode 100644 index 0000000..60c6c77 --- /dev/null +++ b/cmake/rcl_set_symbol_visibility_hidden.cmake @@ -0,0 +1,67 @@ +# 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. + +# +# Configures ros client library with custom settings. +# The custom settings are all related to library symbol visibility, see: +# https://gcc.gnu.org/wiki/Visibility +# http://www.ibm.com/developerworks/aix/library/au-aix-symbol-visibility/ +# +# Below code is heavily referenced from a similar functionality in rmw: +# https://github.com/ros2/rmw/blob/master/rmw/cmake/configure_rmw_library.cmake +# +# :param library_target: the library target +# :type library_target: string +# :param LANGUAGE: Optional flag for the language of the library. +# Allowed values are "C" and "CXX". The default is "CXX". +# :type LANGUAGE: string +# +# @public +# +function(rcl_set_symbol_visibility_hidden library_target) + cmake_parse_arguments(ARG "" "LANGUAGE" "" ${ARGN}) + if(ARG_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "rcl_set_symbol_visibility_hidden() called with unused arguments: ${ARG_UNPARSED_ARGUMENTS}") + endif() + + if(NOT ARG_LANGUAGE) + set(ARG_LANGUAGE "CXX") + endif() + + if(ARG_LANGUAGE STREQUAL "C") + # Set the visibility to hidden by default if possible + if(CMAKE_C_COMPILER_ID STREQUAL "GNU" OR CMAKE_C_COMPILER_ID MATCHES "Clang") + # Set the visibility of symbols to hidden by default for gcc and clang + # (this is already the default on Windows) + set_target_properties(${library_target} + PROPERTIES + COMPILE_FLAGS "-fvisibility=hidden" + ) + endif() + + elseif(ARG_LANGUAGE STREQUAL "CXX") + # Set the visibility to hidden by default if possible + if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # Set the visibility of symbols to hidden by default for gcc and clang + # (this is already the default on Windows) + set_target_properties(${library_target} + PROPERTIES + COMPILE_FLAGS "-fvisibility=hidden -fvisibility-inlines-hidden" + ) + endif() + + else() + message(FATAL_ERROR "rcl_set_symbol_visibility_hidden() called with unsupported LANGUAGE: '${ARG_LANGUAGE}'") + endif() +endfunction() diff --git a/include/rcl/allocator.h b/include/rcl/allocator.h new file mode 100644 index 0000000..c57ec0f --- /dev/null +++ b/include/rcl/allocator.h @@ -0,0 +1,63 @@ +// Copyright 2015 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. + +/// @file + +#ifndef RCL__ALLOCATOR_H_ +#define RCL__ALLOCATOR_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcutils/allocator.h" + +/// Encapsulation of an allocator. +/** + * \sa rcutils_allocator_t + */ +typedef rcutils_allocator_t rcl_allocator_t; + +/// Return a properly initialized rcl_allocator_t with default values. +/** + * \sa rcutils_get_default_allocator() + */ +#define rcl_get_default_allocator rcutils_get_default_allocator + +/// Emulate the behavior of [reallocf](https://linux.die.net/man/3/reallocf). +/** + * \sa rcutils_reallocf() + */ +#define rcl_reallocf rcutils_reallocf + +/// Check that the given allocator is initialized. +/** + * If the allocator is not initialized, run the fail_statement. + */ +#define RCL_CHECK_ALLOCATOR(allocator, fail_statement) \ + RCUTILS_CHECK_ALLOCATOR(allocator, fail_statement) + +/// Check that the given allocator is initialized, or fail with a message. +/** + * If the allocator is not initialized, set the error to msg, and run the fail_statement. + */ +#define RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, msg, fail_statement) \ + RCUTILS_CHECK_ALLOCATOR_WITH_MSG(allocator, msg, fail_statement) + +#ifdef __cplusplus +} +#endif + +#endif // RCL__ALLOCATOR_H_ diff --git a/include/rcl/arguments.h b/include/rcl/arguments.h new file mode 100644 index 0000000..44c99f6 --- /dev/null +++ b/include/rcl/arguments.h @@ -0,0 +1,454 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file + +#ifndef RCL__ARGUMENTS_H_ +#define RCL__ARGUMENTS_H_ + +#include "rcl/allocator.h" +#include "rcl/log_level.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" +#include "rcl_yaml_param_parser/types.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef struct rcl_arguments_impl_s rcl_arguments_impl_t; + +/// Hold output of parsing command line arguments. +typedef struct rcl_arguments_s +{ + /// Private implementation pointer. + rcl_arguments_impl_t * impl; +} rcl_arguments_t; + +/// The command-line flag that delineates the start of ROS arguments. +#define RCL_ROS_ARGS_FLAG "--ros-args" + +/// The token that delineates the explicit end of ROS arguments. +#define RCL_ROS_ARGS_EXPLICIT_END_TOKEN "--" + +/// The ROS flag that precedes the setting of a ROS parameter. +#define RCL_PARAM_FLAG "--param" + +/// The short version of the ROS flag that precedes the setting of a ROS parameter. +#define RCL_SHORT_PARAM_FLAG "-p" + +/// The ROS flag that precedes a path to a file containing ROS parameters. +#define RCL_PARAM_FILE_FLAG "--params-file" + +/// The ROS flag that precedes a ROS remapping rule. +#define RCL_REMAP_FLAG "--remap" + +/// The short version of the ROS flag that precedes a ROS remapping rule. +#define RCL_SHORT_REMAP_FLAG "-r" + +/// The ROS flag that precedes the name of a ROS security enclave. +#define RCL_ENCLAVE_FLAG "--enclave" + +/// The short version of the ROS flag that precedes the name of a ROS security enclave. +#define RCL_SHORT_ENCLAVE_FLAG "-e" + +/// The ROS flag that precedes the ROS logging level to set. +#define RCL_LOG_LEVEL_FLAG "--log-level" + +/// The ROS flag that precedes the name of a configuration file to configure logging. +#define RCL_EXTERNAL_LOG_CONFIG_FLAG "--log-config-file" + +/// The suffix of the ROS flag to enable or disable stdout +/// logging (must be preceded with --enable- or --disable-). +#define RCL_LOG_STDOUT_FLAG_SUFFIX "stdout-logs" + +/// The suffix of the ROS flag to enable or disable rosout +/// logging (must be preceded with --enable- or --disable-). +#define RCL_LOG_ROSOUT_FLAG_SUFFIX "rosout-logs" + +/// The suffix of the ROS flag to enable or disable external library +/// logging (must be preceded with --enable- or --disable-). +#define RCL_LOG_EXT_LIB_FLAG_SUFFIX "external-lib-logs" + +/// Return a rcl_arguments_t struct with members initialized to `NULL`. +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_arguments_t +rcl_get_zero_initialized_arguments(void); + +/// Parse command line arguments into a structure usable by code. +/** + * \sa rcl_get_zero_initialized_arguments() + * + * ROS arguments are expected to be scoped by a leading `--ros-args` flag and a trailing double + * dash token `--` which may be elided if no non-ROS arguments follow after the last `--ros-args`. + * + * Remap rule parsing is supported via `-r/--remap` flags e.g. `--remap from:=to` or `-r from:=to`. + * Successfully parsed remap rules are stored in the order they were given in `argv`. + * If given arguments `{"__ns:=/foo", "__ns:=/bar"}` then the namespace used by nodes in this + * process will be `/foo` and not `/bar`. + * + * \sa rcl_remap_topic_name() + * \sa rcl_remap_service_name() + * \sa rcl_remap_node_name() + * \sa rcl_remap_node_namespace() + * + * Parameter override rule parsing is supported via `-p/--param` flags e.g. `--param name:=value` + * or `-p name:=value`. + * + * The default log level will be parsed as `--log-level level` and logger levels will be parsed as + * multiple `--log-level name:=level`, where `level` is a name representing one of the log levels + * in the `RCUTILS_LOG_SEVERITY` enum, e.g. `info`, `debug`, `warn`, not case sensitive. + * If multiple of these rules are found, the last one parsed will be used. + * + * If an argument does not appear to be a valid ROS argument e.g. a `-r/--remap` flag followed by + * anything but a valid remap rule, parsing will fail immediately. + * + * If an argument does not appear to be a known ROS argument, then it is skipped and left unparsed. + * + * \sa rcl_arguments_get_count_unparsed_ros() + * \sa rcl_arguments_get_unparsed_ros() + * + * All arguments found outside a `--ros-args ... --` scope are skipped and left unparsed. + * + * \sa rcl_arguments_get_count_unparsed() + * \sa rcl_arguments_get_unparsed() + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] argc The number of arguments in argv. + * \param[in] argv The values of the arguments. + * \param[in] allocator A valid allocator. + * \param[out] args_output A structure that will contain the result of parsing. + * Must be zero initialized before use. + * \return #RCL_RET_OK if the arguments were parsed successfully, or + * \return #RCL_RET_INVALID_ROS_ARGS if an invalid ROS argument is found, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_parse_arguments( + int argc, + const char * const * argv, + rcl_allocator_t allocator, + rcl_arguments_t * args_output); + +/// Return the number of arguments that were not ROS specific arguments. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] args An arguments structure that has been parsed. + * \return number of unparsed arguments, or + * \return -1 if args is `NULL` or zero initialized. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +int +rcl_arguments_get_count_unparsed( + const rcl_arguments_t * args); + +/// Return a list of indices to non ROS specific arguments. +/** + * Non ROS specific arguments may have been provided i.e. arguments outside a '--ros-args' scope. + * This function populates an array of indices to these arguments in the original argv array. + * Since the first argument is always assumed to be a process name, the list will always contain + * the index 0. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] args An arguments structure that has been parsed. + * \param[in] allocator A valid allocator. + * \param[out] output_unparsed_indices An allocated array of indices into the original argv array. + * This array must be deallocated by the caller using the given allocator. + * If there are no unparsed args then the output will be set to NULL. + * \return #RCL_RET_OK if everything goes correctly, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_arguments_get_unparsed( + const rcl_arguments_t * args, + rcl_allocator_t allocator, + int ** output_unparsed_indices); + +/// Return the number of ROS specific arguments that were not successfully parsed. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] args An arguments structure that has been parsed. + * \return number of unparsed ROS specific arguments, or + * \return -1 if args is `NULL` or zero initialized. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +int +rcl_arguments_get_count_unparsed_ros( + const rcl_arguments_t * args); + +/// Return a list of indices to unknown ROS specific arguments that were left unparsed. +/** + * Some ROS specific arguments may not have been recognized, or were not intended to be + * parsed by rcl. + * This function populates an array of indices to these arguments in the original argv array. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] args An arguments structure that has been parsed. + * \param[in] allocator A valid allocator. + * \param[out] output_unparsed_ros_indices An allocated array of indices into the original argv array. + * This array must be deallocated by the caller using the given allocator. + * If there are no unparsed ROS specific arguments then the output will be set to NULL. + * \return #RCL_RET_OK if everything goes correctly, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_arguments_get_unparsed_ros( + const rcl_arguments_t * args, + rcl_allocator_t allocator, + int ** output_unparsed_ros_indices); + +/// Return the number of parameter yaml files given in the arguments. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] args An arguments structure that has been parsed. + * \return number of yaml files, or + * \return -1 if args is `NULL` or zero initialized. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +int +rcl_arguments_get_param_files_count( + const rcl_arguments_t * args); + + +/// Return a list of yaml parameter file paths specified on the command line. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] arguments An arguments structure that has been parsed. + * \param[in] allocator A valid allocator. + * \param[out] parameter_files An allocated array of paramter file names. + * This array must be deallocated by the caller using the given allocator. + * The output is NULL if there were no paramter files. + * \return #RCL_RET_OK if everything goes correctly, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_arguments_get_param_files( + const rcl_arguments_t * arguments, + rcl_allocator_t allocator, + char *** parameter_files); + +/// Return all parameter overrides parsed from the command line. +/** + * Parameter overrides are parsed directly from command line arguments and + * parameter files provided in the command line. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] arguments An arguments structure that has been parsed. + * \param[out] parameter_overrides Parameter overrides as parsed from command line arguments. + * This structure must be finalized by the caller. + * The output is NULL if no parameter overrides were parsed. + * \return #RCL_RET_OK if everything goes correctly, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_arguments_get_param_overrides( + const rcl_arguments_t * arguments, + rcl_params_t ** parameter_overrides); + +/// Return a list of arguments with ROS-specific arguments removed. +/** + * Some arguments may not have been intended as ROS arguments. + * This function populates an array of the aruments in a new argv array. + * Since the first argument is always assumed to be a process name, the list + * will always contain the first value from the argument vector. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] argv The argument vector + * \param[in] args An arguments structure that has been parsed. + * \param[in] allocator A valid allocator. + * \param[out] nonros_argc The count of arguments that aren't ROS-specific + * \param[out] nonros_argv An allocated array of arguments that aren't ROS-specific + * This array must be deallocated by the caller using the given allocator. + * If there are no non-ROS args, then the output will be set to NULL. + * \return #RCL_RET_OK if everything goes correctly, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_remove_ros_arguments( + const char * const * argv, + const rcl_arguments_t * args, + rcl_allocator_t allocator, + int * nonros_argc, + const char *** nonros_argv); + +/// Return log levels parsed from the command line. +/** + * Log levels are parsed directly from command line arguments. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] arguments An arguments structure that has been parsed. + * \param[out] log_levels Log levels as parsed from command line arguments. + * The output must be finished by the caller if the function successes. + * \return #RCL_RET_OK if everything goes correctly, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_arguments_get_log_levels( + const rcl_arguments_t * arguments, + rcl_log_levels_t * log_levels); + +/// Copy one arguments structure into another. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] args The structure to be copied. + * Its allocator is used to copy memory into the new structure. + * \param[out] args_out A zero-initialized arguments structure to be copied into. + * \return #RCL_RET_OK if the structure was copied successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_arguments_copy( + const rcl_arguments_t * args, + rcl_arguments_t * args_out); + +/// Reclaim resources held inside rcl_arguments_t structure. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] args The structure to be deallocated. + * \return #RCL_RET_OK if the memory was successfully freed, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_arguments_fini( + rcl_arguments_t * args); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__ARGUMENTS_H_ diff --git a/include/rcl/client.h b/include/rcl/client.h new file mode 100644 index 0000000..224f74b --- /dev/null +++ b/include/rcl/client.h @@ -0,0 +1,500 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file + +#ifndef RCL__CLIENT_H_ +#define RCL__CLIENT_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rosidl_runtime_c/service_type_support_struct.h" + +#include "rcl/event_callback.h" +#include "rcl/macros.h" +#include "rcl/node.h" +#include "rcl/visibility_control.h" + +/// Internal rcl client implementation struct. +typedef struct rcl_client_impl_s rcl_client_impl_t; + +/// Structure which encapsulates a ROS Client. +typedef struct rcl_client_s +{ + /// Pointer to the client implementation + rcl_client_impl_t * impl; +} rcl_client_t; + +/// Options available for a rcl_client_t. +typedef struct rcl_client_options_s +{ + /// Middleware quality of service settings for the client. + rmw_qos_profile_t qos; + /// Custom allocator for the client, used for incidental allocations. + /** For default behavior (malloc/free), use: rcl_get_default_allocator() */ + rcl_allocator_t allocator; +} rcl_client_options_t; + +/// Return a rcl_client_t struct with members set to `NULL`. +/** + * Should be called to get a null rcl_client_t before passing to + * rcl_client_init(). + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_client_t +rcl_get_zero_initialized_client(void); + +/// Initialize a rcl client. +/** + * After calling this function on a rcl_client_t, it can be used to send + * requests of the given type by calling rcl_send_request(). + * If the request is received by a (possibly remote) service and if the service + * sends a response, the client can access the response through + * rcl_take_response() once the response is available to the client. + * + * The given rcl_node_t must be valid and the resulting rcl_client_t is only + * valid as long as the given rcl_node_t remains valid. + * + * The rosidl_service_type_support_t is obtained on a per `.srv` type basis. + * When the user defines a ROS service, code is generated which provides the + * required rosidl_service_type_support_t object. + * This object can be obtained using a language appropriate mechanism. + * \todo TODO(wjwwood) write these instructions once and link to it instead + * + * For C, a macro can be used (for example `example_interfaces/AddTwoInts`): + * + * ```c + * #include + * #include + * + * const rosidl_service_type_support_t * ts = + * ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts); + * ``` + * + * For C++, a template function is used: + * + * ```cpp + * #include + * #include + * + * using rosidl_typesupport_cpp::get_service_type_support_handle; + * const rosidl_service_type_support_t * ts = + * get_service_type_support_handle(); + * ``` + * + * The rosidl_service_type_support_t object contains service type specific + * information used to send or take requests and responses. + * + * The topic name must be a c string which follows the topic and service name + * format rules for unexpanded names, also known as non-fully qualified names: + * + * \see rcl_expand_topic_name + * + * The options struct allows the user to set the quality of service settings as + * well as a custom allocator which is used when initializing/finalizing the + * client to allocate space for incidentals, e.g. the service name string. + * + * Expected usage (for C services): + * + * ```c + * #include + * #include + * #include + * + * rcl_node_t node = rcl_get_zero_initialized_node(); + * rcl_node_options_t node_ops = rcl_node_get_default_options(); + * rcl_ret_t ret = rcl_node_init(&node, "node_name", "/my_namespace", &node_ops); + * // ... error handling + * const rosidl_service_type_support_t * ts = + * ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts); + * rcl_client_t client = rcl_get_zero_initialized_client(); + * rcl_client_options_t client_ops = rcl_client_get_default_options(); + * ret = rcl_client_init(&client, &node, ts, "add_two_ints", &client_ops); + * // ... error handling, and on shutdown do finalization: + * ret = rcl_client_fini(&client, &node); + * // ... error handling for rcl_client_fini() + * ret = rcl_node_fini(&node); + * // ... error handling for rcl_node_fini() + * ``` + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] client preallocated rcl_client_t structure + * \param[in] node valid rcl_node_t + * \param[in] type_support type support object for the service's type + * \param[in] service_name the name of the service to request + * \param[in] options client options, including quality of service settings + * \return #RCL_RET_OK if the client was initialized successfully, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_ALREADY_INIT if the client is already initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory fails, or + * \return #RCL_RET_SERVICE_NAME_INVALID if the given service name is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_client_init( + rcl_client_t * client, + const rcl_node_t * node, + const rosidl_service_type_support_t * type_support, + const char * service_name, + const rcl_client_options_t * options); + +/// Finalize a rcl_client_t. +/** + * After calling this function, calls to rcl_send_request() and + * rcl_take_response() will fail when using this client. + * However, the given node handle is still valid. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] client handle to the client to be finalized + * \param[in] node a valid (not finalized) handle to the node used to create the client + * \return #RCL_RET_OK if client was finalized successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_client_fini(rcl_client_t * client, rcl_node_t * node); + +/// Return the default client options in a rcl_client_options_t. +/** + * The defaults are: + * + * - qos = rmw_qos_profile_services_default + * - allocator = rcl_get_default_allocator() + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_client_options_t +rcl_client_get_default_options(void); + +/// Send a ROS request using a client. +/** + * It is the job of the caller to ensure that the type of the `ros_request` + * parameter and the type associate with the client (via the type support) + * match. + * Passing a different type to `send_request` produces undefined behavior and + * cannot be checked by this function and therefore no deliberate error will + * occur. + * + * rcl_send_request() is an non-blocking call. + * + * The ROS request message given by the `ros_request` void pointer is always + * owned by the calling code, but should remain constant during `send_request`. + * + * This function is thread safe so long as access to both the client and the + * `ros_request` is synchronized. + * That means that calling rcl_send_request() from multiple threads is allowed, + * but calling rcl_send_request() at the same time as non-thread safe client + * functions is not, e.g. calling rcl_send_request() and rcl_client_fini() + * concurrently is not allowed. + * Before calling rcl_send_request() the message can change and after calling + * rcl_send_request() the message can change, but it cannot be changed during + * the `send_request` call. + * The same `ros_request`, however, can be passed to multiple calls of + * rcl_send_request() simultaneously, even if the clients differ. + * The `ros_request` is unmodified by rcl_send_request(). + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes [1] + * Uses Atomics | No + * Lock-Free | Yes + * [1] for unique pairs of clients and requests, see above for more + * + * \param[in] client handle to the client which will make the response + * \param[in] ros_request type-erased pointer to the ROS request message + * \param[out] sequence_number the sequence number + * \return #RCL_RET_OK if the request was sent successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_CLIENT_INVALID if the client is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t * sequence_number); + + +/// Take a ROS response using a client +/** + * It is the job of the caller to ensure that the type of the `ros_response` + * parameter and the type associate with the client (via the type support) + * match. + * Passing a different type to take_response produces undefined behavior and + * cannot be checked by this function and therefore no deliberate error will + * occur. + * The request_header is an rmw struct for meta-information about the request + * sent (e.g. the sequence number). + * The caller must provide a pointer to an allocated struct. + * This function will populate the struct's fields. + * `ros_response` should point to an already allocated ROS response message + * struct of the correct type, into which the response from the service will be + * copied. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Maybe [1] + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * [1] only if required when filling the message, avoided for fixed sizes + * + * \param[in] client handle to the client which will take the response + * \param[inout] request_header pointer to the request header + * \param[inout] ros_response type-erased pointer to the ROS response message + * \return #RCL_RET_OK if the response was taken successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_CLIENT_INVALID if the client is invalid, or + * \return #RCL_RET_CLIENT_TAKE_FAILED if take failed but no error occurred + * in the middleware, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_response_with_info( + const rcl_client_t * client, + rmw_service_info_t * request_header, + void * ros_response); + +/// backwards compatibility function that takes a rmw_request_id_t only +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_response( + const rcl_client_t * client, + rmw_request_id_t * request_header, + void * ros_response); + +/// Get the name of the service that this client will request a response from. +/** + * This function returns the client's internal service name string. + * This function can fail, and therefore return `NULL`, if the: + * - client is `NULL` + * - client is invalid (never called init, called fini, or invalid node) + * + * The returned string is only valid as long as the rcl_client_t is valid. + * The value of the string may change if the service name changes, and therefore + * copying the string is recommended if this is a concern. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] client pointer to the client + * \return name string if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_client_get_service_name(const rcl_client_t * client); + +/// Return the rcl client options. +/** + * This function returns the client's internal options struct. + * This function can fail, and therefore return `NULL`, if the: + * - client is `NULL` + * - client is invalid (never called init, called fini, or invalid node) + * + * The returned struct is only valid as long as the rcl_client_t is valid. + * The values in the struct may change if the options of the client change, + * and therefore copying the struct is recommended if this is a concern. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] client pointer to the client + * \return options struct if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rcl_client_options_t * +rcl_client_get_options(const rcl_client_t * client); + +/// Return the rmw client handle. +/** + * The handle returned is a pointer to the internally held rmw handle. + * This function can fail, and therefore return `NULL`, if the: + * - client is `NULL` + * - client is invalid (never called init, called fini, or invalid node) + * + * The returned handle is made invalid if the client is finalized or if + * rcl_shutdown() is called. + * The returned handle is not guaranteed to be valid for the life time of the + * client as it may be finalized and recreated itself. + * Therefore it is recommended to get the handle from the client using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] client pointer to the rcl client + * \return rmw client handle if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_client_t * +rcl_client_get_rmw_handle(const rcl_client_t * client); + +/// Check that the client is valid. +/** + * The bool returned is `false` if client is invalid. + * The bool returned is `true` otherwise. + * In the case where `false` is to be returned, an error message is set. + * This function cannot fail. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] client pointer to the rcl client + * \return `true` if `client` is valid, otherwise `false` + */ +RCL_PUBLIC +bool +rcl_client_is_valid(const rcl_client_t * client); + +/// Get the actual qos settings of the client's request publisher. +/** + * Used to get the actual qos settings of the client's request publisher. + * The actual configuration applied when using RMW_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the client, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_*_UNKNOWN. + * The returned struct is only valid as long as the rcl_client_t is valid. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] client pointer to the rcl client + * \return qos struct if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rmw_qos_profile_t * +rcl_client_request_publisher_get_actual_qos(const rcl_client_t * client); + +/// Get the actual qos settings of the client's response subscription. +/** + * Used to get the actual qos settings of the client's response subscription. + * The actual configuration applied when using RMW_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the client, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_*_UNKNOWN. + * The returned struct is only valid as long as the rcl_client_t is valid. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] client pointer to the rcl client + * \return qos struct if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rmw_qos_profile_t * +rcl_client_response_subscription_get_actual_qos(const rcl_client_t * client); + +/// Set the on new response callback function for the client. +/** + * This API sets the callback function to be called whenever the + * client is notified about a new response. + * + * \sa rmw_client_set_on_new_response_callback for details about this function. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] rmw implementation defined + * + * \param[in] client The client on which to set the callback + * \param[in] callback The callback to be called when new responses arrive, may be NULL + * \param[in] user_data Given to the callback when called later, may be NULL + * \return `RCL_RET_OK` if callback was set to the listener, or + * \return `RCL_RET_INVALID_ARGUMENT` if `client` is NULL, or + * \return `RCL_RET_UNSUPPORTED` if the API is not implemented in the dds implementation + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_client_set_on_new_response_callback( + const rcl_client_t * client, + rcl_event_callback_t callback, + const void * user_data); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__CLIENT_H_ diff --git a/include/rcl/context.h b/include/rcl/context.h new file mode 100644 index 0000000..68500da --- /dev/null +++ b/include/rcl/context.h @@ -0,0 +1,321 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file + +#ifndef RCL__CONTEXT_H_ +#define RCL__CONTEXT_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rmw/init.h" + +#include "rcl/allocator.h" +#include "rcl/arguments.h" +#include "rcl/init_options.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +/// @cond Doxygen_Suppress +#ifdef _MSC_VER +#define RCL_ALIGNAS(N) __declspec(align(N)) +#else +#include +#define RCL_ALIGNAS(N) alignas(N) +#endif +/// @endcond + +/// A unique ID per context instance. +typedef uint64_t rcl_context_instance_id_t; + +typedef struct rcl_context_impl_s rcl_context_impl_t; + +/// Encapsulates the non-global state of an init/shutdown cycle. +/** + * The context is used in the creation of top level entities like nodes and + * guard conditions, as well as to shutdown a specific instance of init. + * + * Here is a diagram of a typical context's lifecycle: + * + * ``` + * +---------------+ + * | | + * +--> uninitialized +---> rcl_get_zero_initialized_context() + + * | | | | + * | +---------------+ | + * | | + * | +-----------------------------------------------+ + * | | + * | +--------v---------+ +-----------------------+ + * | | | | | + * | | zero-initialized +-> rcl_init() +-> initialized and valid +-> rcl_shutdown() + + * | | | | | | + * | +------------------+ +-----------------------+ | + * | | + * | +-----------------------------------------------------------------+ + * | | + * | +------------v------------+ + * | | | + * | | initialized but invalid +---> finalize all entities, then rcl_context_fini() + + * | | | | + * | +-------------------------+ | + * | | + * +---------------------------------------------------------------------------------+ + * ``` + * + * A declared but not defined rcl_context_t instance is considered to be + * "uninitialized", and passing an uninitialized context to any functions will + * result in undefined behavior. + * Some functions, like rcl_init() require the context instance to be + * zero initialized (all members set to "zero" state) before use. + * + * Zero initialization of an rcl_context_t should be done with + * rcl_get_zero_initialized_context(), which ensures the context is in a safe + * state for initialization with rcl_init(). + * + * Initialization of an rcl_context_t should be done with rcl_init(), after + * which the context is considered both initialized and valid. + * After initialization it can be used in the creation of other entities like + * nodes and guard conditions. + * + * At any time the context can be invalidated by calling rcl_shutdown() on + * the rcl_context_t, after which the context is still initialized but now + * invalid. + * + * Invalidation indicates to other entities that the context was shutdown, but + * is still accessible for use during cleanup of themselves. + * + * After being invalidated, and after all of the entities which used it have + * been finalized, the context should be finalized with rcl_context_fini(). + * + * Finalizing the context while entities which have copies of it have not yet + * been finalized is undefined behavior. + * Therefore, the context's lifetime (between calls to rcl_init() and + * rcl_context_fini()) should exceed the lifetime of all entities which use + * it directly (e.g. nodes and guard conditions) or indirectly (e.g. + * subscriptions and topics). + */ +typedef struct rcl_context_s +{ + /// Global arguments for all nodes which share this context. + /** Typically generated by the parsing of argc/argv in rcl_init(). */ + rcl_arguments_t global_arguments; + + /// Implementation specific pointer. + rcl_context_impl_t * impl; + + // The assumption that this is big enough for an atomic_uint_least64_t is + // ensured with a static_assert in the context.c file. + // In most cases it should just be a plain uint64_t. +/// @cond Doxygen_Suppress +#if !defined(RCL_CONTEXT_ATOMIC_INSTANCE_ID_STORAGE_SIZE) +#define RCL_CONTEXT_ATOMIC_INSTANCE_ID_STORAGE_SIZE sizeof(uint_least64_t) +#endif +/// @endcond + /// Private storage for instance ID atomic. + /** + * Accessing the instance id should be done using the function + * rcl_context_get_instance_id() because the instance id's type is an + * atomic and needs to be accessed properly to ensure safety. + * + * The instance id should not be changed manually - doing so is undefined + * behavior. + * + * The instance id cannot be protected within the `impl` pointer's type + * because it needs to be accessible even when the context is zero + * initialized and therefore `impl` is `NULL`. + * Specifically, storing the instance id in the `impl` would introduce a + * race condition between accessing it and finalizing the context. + * Additionally, C11 atomics (i.e. "stdatomic.h") cannot be used directly + * here in the case that this header is included into a C++ program. + * See this paper for an effort to make this possible in the future: + * http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2018/p0943r1.html + */ + RCL_ALIGNAS(8) uint8_t instance_id_storage[RCL_CONTEXT_ATOMIC_INSTANCE_ID_STORAGE_SIZE]; +} rcl_context_t; + +/// Return a zero initialization context object. +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_context_t +rcl_get_zero_initialized_context(void); + +/// Finalize a context. +/** + * The context to be finalized must have been previously initialized with + * rcl_init(), and then later invalidated with rcl_shutdown(). + * A zero-initialized context that has not been initialized can be finalized. + * If context is `NULL`, then #RCL_RET_INVALID_ARGUMENT is returned. + * If context is zero-initialized, then #RCL_RET_OK is returned. + * If context is initialized and valid (rcl_shutdown() was not called on it), + * then #RCL_RET_INVALID_ARGUMENT is returned. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_uint_least64_t` + * + * \param[inout] context object to be finalized. + * \return #RCL_RET_OK if the shutdown was completed successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_context_fini(rcl_context_t * context); + +/// Return the init options used during initialization for this context. +/** + * This function can fail and return `NULL` if: + * - context is NULL + * - context is zero-initialized, e.g. context->impl is `NULL` + * + * If context is uninitialized then that is undefined behavior. + * + * If `NULL` is returned an error message will have been set. + * + * The options are for reference only, and therefore the returned pointer is + * const. + * Changing the values in the options is undefined behavior but will likely + * have no effect. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes + * + * \param[in] context object from which the init options should be retrieved + * \return pointer to the the init options, or + * \return `NULL` if there was an error + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rcl_init_options_t * +rcl_context_get_init_options(const rcl_context_t * context); + +/// Returns an unsigned integer that is unique to the given context, or `0` if invalid. +/** + * The given context must be non-`NULL`, but does not need to be initialized or valid. + * If context is `NULL`, then `0` will be returned. + * If context is uninitialized, then it is undefined behavior. + * + * The instance ID may be `0` if the context is zero-initialized or if the + * context has been invalidated by rcl_shutdown(). + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_uint_least64_t` + * + * \param[in] context object from which the instance id should be retrieved + * \return a unique id specific to this context instance, or + * \return `0` if invalid, or + * \return `0` if context is `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_context_instance_id_t +rcl_context_get_instance_id(const rcl_context_t * context); + +/// Returns the context domain id. +/** + * \pre If context is uninitialized, then it is undefined behavior. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes [1] + * Uses Atomics | No + * Lock-Free | No + * + * [1] Calling the function asynchronously with rcl_init() or rcl_shutdown() can result + * in the function sometimes succeeding and sometimes returning #RCL_RET_INVALID_ARGUMENT. + * + * \param[in] context from which the domain id should be retrieved. + * \param[out] domain_id output variable where the domain id will be returned. + * \return #RCL_RET_INVALID_ARGUMENT if `context` is invalid (see rcl_context_is_valid()), or + * \return #RCL_RET_INVALID_ARGUMENT if `domain_id` is `NULL`, or + * \return #RCL_RET_OK if the domain id was correctly retrieved. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_context_get_domain_id(rcl_context_t * context, size_t * domain_id); + +/// Return `true` if the given context is currently valid, otherwise `false`. +/** + * If context is `NULL`, then `false` is returned. + * If context is zero-initialized, then `false` is returned. + * If context is uninitialized, then it is undefined behavior. + * + * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_uint_least64_t` + * + * \param[in] context object which should be checked for validity + * \return `true` if valid, otherwise `false` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +bool +rcl_context_is_valid(const rcl_context_t * context); + +/// Return pointer to the rmw context if the given context is currently valid, otherwise `NULL`. +/** + * If context is `NULL`, then `NULL` is returned. + * If context is zero-initialized, then `NULL` is returned. + * If context is uninitialized, then it is undefined behavior. + * + * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_uint_least64_t` + * + * \param[in] context object from which the rmw context should be retrieved. + * \return pointer to rmw context if valid, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_context_t * +rcl_context_get_rmw_context(rcl_context_t * context); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__CONTEXT_H_ diff --git a/include/rcl/domain_id.h b/include/rcl/domain_id.h new file mode 100644 index 0000000..946c683 --- /dev/null +++ b/include/rcl/domain_id.h @@ -0,0 +1,51 @@ +// 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. + +/// @file + +#ifndef RCL__DOMAIN_ID_H_ +#define RCL__DOMAIN_ID_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +#include "rcl/types.h" +#include "rcl/visibility_control.h" +#include "rmw/domain_id.h" + +/// The default domain ID used by RCL. +#define RCL_DEFAULT_DOMAIN_ID RMW_DEFAULT_DOMAIN_ID + +extern const char * const RCL_DOMAIN_ID_ENV_VAR; + +/// Determine the default domain ID, based on the environment. +/** + * \param[out] domain_id Must not be NULL. + * \returns #RCL_RET_INVALID_ARGUMENT if an argument is invalid, or, + * \returns #RCL_RET_ERROR in case of an unexpected error, or, + * \returns #RCL_RET_OK. + */ +RCL_PUBLIC +rcl_ret_t +rcl_get_default_domain_id(size_t * domain_id); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__DOMAIN_ID_H_ diff --git a/include/rcl/error_handling.h b/include/rcl/error_handling.h new file mode 100644 index 0000000..f06c892 --- /dev/null +++ b/include/rcl/error_handling.h @@ -0,0 +1,49 @@ +// Copyright 2015 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 RCL__ERROR_HANDLING_H_ +#define RCL__ERROR_HANDLING_H_ + +#include "rcutils/error_handling.h" + +/// The error handling in RCL is just an alias to the error handling in rcutils. + +typedef rcutils_error_state_t rcl_error_state_t; +typedef rcutils_error_string_t rcl_error_string_t; + +#define rcl_initialize_error_handling_thread_local_storage \ + rcutils_initialize_error_handling_thread_local_storage + +#define rcl_set_error_state rcutils_set_error_state + +#define RCL_CHECK_ARGUMENT_FOR_NULL(argument, error_return_type) \ + RCUTILS_CHECK_ARGUMENT_FOR_NULL(argument, error_return_type) + +#define RCL_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement) \ + RCUTILS_CHECK_FOR_NULL_WITH_MSG(value, msg, error_statement) + +#define RCL_SET_ERROR_MSG(msg) RCUTILS_SET_ERROR_MSG(msg) + +#define RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(fmt_str, ...) \ + RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING(fmt_str, __VA_ARGS__) + +#define rcl_error_is_set rcutils_error_is_set + +#define rcl_get_error_state rcutils_get_error_state + +#define rcl_get_error_string rcutils_get_error_string + +#define rcl_reset_error rcutils_reset_error + +#endif // RCL__ERROR_HANDLING_H_ diff --git a/include/rcl/event.h b/include/rcl/event.h new file mode 100644 index 0000000..fea87d7 --- /dev/null +++ b/include/rcl/event.h @@ -0,0 +1,237 @@ +// 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. + +/// @file + +#ifndef RCL__EVENT_H_ +#define RCL__EVENT_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +#include "rcl/client.h" +#include "rcl/event_callback.h" +#include "rcl/macros.h" +#include "rcl/publisher.h" +#include "rcl/service.h" +#include "rcl/subscription.h" +#include "rcl/visibility_control.h" + +/// Enumeration of all of the publisher events that may fire. +typedef enum rcl_publisher_event_type_e +{ + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, + RCL_PUBLISHER_LIVELINESS_LOST, + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, +} rcl_publisher_event_type_t; + +/// Enumeration of all of the subscription events that may fire. +typedef enum rcl_subscription_event_type_e +{ + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED, + RCL_SUBSCRIPTION_LIVELINESS_CHANGED, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS, + RCL_SUBSCRIPTION_MESSAGE_LOST, +} rcl_subscription_event_type_t; + +/// Internal rcl implementation struct. +typedef struct rcl_event_impl_s rcl_event_impl_t; + +/// Structure which encapsulates a ROS QoS event handle. +typedef struct rcl_event_s +{ + /// Pointer to the event implementation + rcl_event_impl_t * impl; +} rcl_event_t; + +/// Return a rcl_event_t struct with members set to `NULL`. +/** + * Should be called to get a null rcl_event_t before passing to + * rcl_event_init(). + * + * \return Zero initialized rcl_event_t. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_event_t +rcl_get_zero_initialized_event(void); + +/// Initialize an rcl_event_t with a publisher. +/** + * Fill the rcl_event_t with the publisher and desired event_type. + * + * \param[in,out] event pointer to fill + * \param[in] publisher to get events from + * \param[in] event_type to listen for + * \return #RCL_RET_OK if the rcl_event_t is filled, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory fails, or + * \return #RCL_RET_UNSUPPORTED if event_type is not supported, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_publisher_event_init( + rcl_event_t * event, + const rcl_publisher_t * publisher, + const rcl_publisher_event_type_t event_type); + +/// Initialize an rcl_event_t with a subscription. +/** + * Fill the rcl_event_t with the subscription and desired event_type. + * + * \param[in,out] event pointer to fill + * \param[in] subscription to get events from + * \param[in] event_type to listen for + * \return #RCL_RET_OK if the rcl_event_t is filled, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory fails, or + * \return #RCL_RET_UNSUPPORTED if event_type is not supported, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_event_init( + rcl_event_t * event, + const rcl_subscription_t * subscription, + const rcl_subscription_event_type_t event_type); + +// Take event using the event handle. +/** + * Take an event from the event handle. + * + * \param[in] event event object to take from + * \param[in, out] event_info event info object to write taken data into + * \return #RCL_RET_OK if successful, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if memory allocation failed, or + * \return #RCL_RET_EVENT_TAKE_FAILED if the take event failed, or + * \return #RCL_RET_ERROR if an unexpected error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_event( + const rcl_event_t * event, + void * event_info); + +// Finalize an event. +/** + * Finalize an event. + * + * \param[in] event to finalize + * \return #RCL_RET_OK if successful, or + * \return #RCL_RET_EVENT_INVALID if event is null, or + * \return #RCL_RET_ERROR if an unexpected error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_event_fini(rcl_event_t * event); + +/// Return the rmw event handle. +/** + * The handle returned is a pointer to the internally held rmw handle. + * This function can fail, and therefore return `NULL`, if the: + * - event is `NULL` + * - event is invalid (never called init, called fini, or invalid node) + * + * The returned handle is made invalid if the event is finalized or if + * rcl_shutdown() is called. + * The returned handle is not guaranteed to be valid for the life time of the + * event as it may be finalized and recreated itself. + * Therefore it is recommended to get the handle from the event using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] event pointer to the rcl event + * \return rmw event handle if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_event_t * +rcl_event_get_rmw_handle(const rcl_event_t * event); + +/// Check that the event is valid. +/** + * The bool returned is `false` if `event` is invalid. + * The bool returned is `true` otherwise. + * In the case where `false` is to be returned, an error message is set. + * This function cannot fail. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] event pointer to the rcl event + * \return `true` if `event` is valid, otherwise `false` + */ +RCL_PUBLIC +bool +rcl_event_is_valid(const rcl_event_t * event); + +/// Set the callback function for the event. +/** + * This API sets the callback function to be called whenever the + * event is notified about a new instance of the event. + * + * \sa rmw_event_set_callback for more details about this function. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] rmw implementation defined + * + * \param[in] event The event on which to set the callback + * \param[in] callback The callback to be called when new events occur, may be NULL + * \param[in] user_data Given to the callback when called later, may be NULL + * \return `RCL_RET_OK` if callback was set to the listener, or + * \return `RCL_RET_INVALID_ARGUMENT` if `event` is NULL, or + * \return `RCL_RET_UNSUPPORTED` if the API is not implemented in the dds implementation + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_event_set_callback( + const rcl_event_t * event, + rcl_event_callback_t callback, + const void * user_data); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__EVENT_H_ diff --git a/include/rcl/event_callback.h b/include/rcl/event_callback.h new file mode 100644 index 0000000..9124907 --- /dev/null +++ b/include/rcl/event_callback.h @@ -0,0 +1,31 @@ +// 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 RCL__EVENT_CALLBACK_H_ +#define RCL__EVENT_CALLBACK_H_ + +#include "rmw/event_callback_type.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef rmw_event_callback_t rcl_event_callback_t; + +#ifdef __cplusplus +} +#endif + +#endif // RCL__EVENT_CALLBACK_H_ diff --git a/include/rcl/expand_topic_name.h b/include/rcl/expand_topic_name.h new file mode 100644 index 0000000..058e57e --- /dev/null +++ b/include/rcl/expand_topic_name.h @@ -0,0 +1,146 @@ +// 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. + +/// @file + +#ifndef RCL__EXPAND_TOPIC_NAME_H_ +#define RCL__EXPAND_TOPIC_NAME_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcutils/types/string_map.h" +#include "rcl/allocator.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +/// Expand a given topic name into a fully-qualified topic name. +/** + * The input_topic_name, node_name, and node_namespace arguments must all be + * valid, null terminated c strings. + * The output_topic_name will not be assigned a value in the event of an error. + * + * The output_topic_name will be null terminated. + * It is also allocated, so it needs to be deallocated, when it is no longer + * needed, with the same allocator given to this function. + * Make sure the `char *` which is passed for the output_topic_name does not + * point to allocated memory before calling this function, because it will be + * overwritten and therefore leaked if this function is successful. + * + * Expected usage: + * + * ```c + * rcl_allocator_t allocator = rcl_get_default_allocator(); + * rcutils_allocator_t rcutils_allocator = rcutils_get_default_allocator(); + * rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map(); + * rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, rcutils_allocator); + * if (rcutils_ret != RCUTILS_RET_OK) { + * // ... error handling + * } + * rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); + * if (ret != RCL_RET_OK) { + * // ... error handling + * } + * char * expanded_topic_name = NULL; + * ret = rcl_expand_topic_name( + * "some/topic", + * "my_node", + * "my_ns", + * &substitutions_map, + * allocator, + * &expanded_topic_name); + * if (ret != RCL_RET_OK) { + * // ... error handling + * } else { + * RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Expanded topic name: %s", expanded_topic_name) + * // ... when done the output topic name needs to be deallocated: + * allocator.deallocate(expanded_topic_name, allocator.state); + * } + * ``` + * + * The input topic name is validated using rcl_validate_topic_name(), + * but if it fails validation RCL_RET_TOPIC_NAME_INVALID is returned. + * + * The input node name is validated using rmw_validate_node_name(), + * but if it fails validation RCL_RET_NODE_INVALID_NAME is returned. + * + * The input node namespace is validated using rmw_validate_namespace(), + * but if it fails validation RCL_RET_NODE_INVALID_NAMESPACE is returned. + * + * In addition to what is given by rcl_get_default_topic_name_substitutions(), + * there are there these substitutions: + * + * - {node} -> the name of the node + * - {namespace} -> the namespace of the node + * - {ns} -> the namespace of the node + * + * If an unknown substitution is used, RCL_RET_UNKNOWN_SUBSTITUTION is returned. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] input_topic_name topic name to be expanded + * \param[in] node_name name of the node associated with the topic + * \param[in] node_namespace namespace of the node associated with the topic + * \param[in] substitutions string map with possible substitutions + * \param[in] allocator the allocator to be used when creating the output topic + * \param[out] output_topic_name output char * pointer + * \return #RCL_RET_OK if the topic name was expanded successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_TOPIC_NAME_INVALID if the given topic name is invalid, or + * \return #RCL_RET_NODE_INVALID_NAME if the name is invalid, or + * \return #RCL_RET_NODE_INVALID_NAMESPACE if the namespace_ is invalid, or + * \return #RCL_RET_UNKNOWN_SUBSTITUTION for unknown substitutions in name, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_expand_topic_name( + const char * input_topic_name, + const char * node_name, + const char * node_namespace, + const rcutils_string_map_t * substitutions, + rcl_allocator_t allocator, + char ** output_topic_name); + +/// Fill a given string map with the default substitution pairs. +/** + * If the string map is not initialized RCL_RET_INVALID_ARGUMENT is returned. + * + * \param[inout] string_map rcutils_string_map_t map to be filled with pairs + * \return #RCL_RET_OK if successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_default_topic_name_substitutions(rcutils_string_map_t * string_map); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__EXPAND_TOPIC_NAME_H_ diff --git a/include/rcl/graph.h b/include/rcl/graph.h new file mode 100644 index 0000000..1c17762 --- /dev/null +++ b/include/rcl/graph.h @@ -0,0 +1,846 @@ +// Copyright 2016-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. + +/// @file + +#ifndef RCL__GRAPH_H_ +#define RCL__GRAPH_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include + +#include "rcutils/time.h" +#include "rcutils/types.h" + +#include "rosidl_runtime_c/service_type_support_struct.h" + +#include "rcl/macros.h" +#include "rcl/client.h" +#include "rcl/node.h" +#include "rcl/visibility_control.h" + +/// A structure that contains topic names and types. +typedef rmw_names_and_types_t rcl_names_and_types_t; + +/// A structure that encapsulates the node name, node namespace, +/// topic type, gid, and qos_profile or publishers and subscriptions +/// for a topic. +typedef rmw_topic_endpoint_info_t rcl_topic_endpoint_info_t; + +/// An array of topic endpoint information. +typedef rmw_topic_endpoint_info_array_t rcl_topic_endpoint_info_array_t; + +/// Return a zero-initialized rcl_names_and_types_t structure. +#define rcl_get_zero_initialized_names_and_types rmw_get_zero_initialized_names_and_types + +/// Return a zero-initialized rcl_topic_endpoint_info_t structure. +#define rcl_get_zero_initialized_topic_endpoint_info_array \ + rmw_get_zero_initialized_topic_endpoint_info_array + +/// Finalize a topic_endpoint_info_array_t structure. +#define rcl_topic_endpoint_info_array_fini rmw_topic_endpoint_info_array_fini + +/// Return a list of topic names and types for publishers associated with a node. +/** + * The `node` parameter must point to a valid node. + * + * The `topic_names_and_types` parameter must be allocated and zero initialized. + * This function allocates memory for the returned list of names and types and so it is the + * callers responsibility to pass `topic_names_and_types` to rcl_names_and_types_fini() + * when it is no longer needed. + * Failing to do so will result in leaked memory. + * + * There may be some demangling that occurs when listing the names from the middleware + * implementation. + * If the `no_demangle` argument is set to `true`, then this will be avoided and the names will be + * returned as they appear to the middleware. + * + * \see rmw_get_topic_names_and_types for more details on no_demangle + * + * The returned names are not automatically remapped by this function. + * Attempting to create publishers or subscribers using names returned by this function may not + * result in the desired topic name being used depending on the remap rules in use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator allocator to be used when allocating space for strings + * \param[in] no_demangle if true, list all topics without any demangling + * \param[in] node_name the node name of the topics to return + * \param[in] node_namespace the node namespace of the topics to return + * \param[out] topic_names_and_types list of topic names and their types + * \return #RCL_RET_OK if the query was successful, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID_NAME if the node name is invalid, or + * \return #RCL_RET_NODE_INVALID_NAMESPACE if the node namespace is invalid, or + * \return #RCL_RET_NODE_NAME_NON_EXISTENT if the node name wasn't found, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_publisher_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + bool no_demangle, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * topic_names_and_types); + +/// Return a list of topic names and types for subscriptions associated with a node. +/** + * The `node` parameter must point to a valid node. + * + * The `topic_names_and_types` parameter must be allocated and zero initialized. + * This function allocates memory for the returned list of names and types and so it is the + * callers responsibility to pass `topic_names_and_types` to rcl_names_and_types_fini() + * when it is no longer needed. + * Failing to do so will result in leaked memory. + * + * \see rcl_get_publisher_names_and_types_by_node for details on the `no_demangle` parameter. + * + * The returned names are not automatically remapped by this function. + * Attempting to create publishers or subscribers using names returned by this function may not + * result in the desired topic name being used depending on the remap rules in use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator allocator to be used when allocating space for strings + * \param[in] no_demangle if true, list all topics without any demangling + * \param[in] node_name the node name of the topics to return + * \param[in] node_namespace the node namespace of the topics to return + * \param[out] topic_names_and_types list of topic names and their types + * \return #RCL_RET_OK if the query was successful, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID_NAME if the node name is invalid, or + * \return #RCL_RET_NODE_INVALID_NAMESPACE if the node namespace is invalid, or + * \return #RCL_RET_NODE_NAME_NON_EXISTENT if the node name wasn't found, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_subscriber_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + bool no_demangle, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * topic_names_and_types); + +/// Return a list of service names and types associated with a node. +/** + * The `node` parameter must point to a valid node. + * + * The `service_names_and_types` parameter must be allocated and zero initialized. + * This function allocates memory for the returned list of names and types and so it is the + * callers responsibility to pass `service_names_and_types` to rcl_names_and_types_fini() + * when it is no longer needed. + * Failing to do so will result in leaked memory. + * + * \see rcl_get_publisher_names_and_types_by_node for details on the `no_demangle` parameter. + * + * The returned names are not automatically remapped by this function. + * Attempting to create service clients using names returned by this function may not + * result in the desired service name being used depending on the remap rules in use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator allocator to be used when allocating space for strings + * \param[in] node_name the node name of the services to return + * \param[in] node_namespace the node namespace of the services to return + * \param[out] service_names_and_types list of service names and their types + * \return #RCL_RET_OK if the query was successful, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID_NAME if the node name is invalid, or + * \return #RCL_RET_NODE_INVALID_NAMESPACE if the node namespace is invalid, or + * \return #RCL_RET_NODE_NAME_NON_EXISTENT if the node name wasn't found, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_service_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * service_names_and_types); + +/// Return a list of service client names and types associated with a node. +/** + * The `node` parameter must point to a valid node. + * + * The `service_names_and_types` parameter must be allocated and zero initialized. + * This function allocates memory for the returned list of names and types and so it is the + * callers responsibility to pass `service_names_and_types` to rcl_names_and_types_fini() + * when it is no longer needed. + * Failing to do so will result in leaked memory. + * + * \see rcl_get_publisher_names_and_types_by_node for details on the `no_demangle` parameter. + * + * The returned names are not automatically remapped by this function. + * Attempting to create service servers using names returned by this function may not + * result in the desired service name being used depending on the remap rules in use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator allocator to be used when allocating space for strings + * \param[in] node_name the node name of the services to return + * \param[in] node_namespace the node namespace of the services to return + * \param[out] service_names_and_types list of service client names and their types + * \return #RCL_RET_OK if the query was successful, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID_NAME if the node name is invalid, or + * \return #RCL_RET_NODE_INVALID_NAMESPACE if the node namespace is invalid, or + * \return #RCL_RET_NODE_NAME_NON_EXISTENT if the node name wasn't found, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_client_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * service_names_and_types); + +/// Return a list of topic names and their types. +/** + * The `node` parameter must point to a valid node. + * + * The `topic_names_and_types` parameter must be allocated and zero initialized. + * This function allocates memory for the returned list of names and types and so it is the + * callers responsibility to pass `topic_names_and_types` to rcl_names_and_types_fini() + * when it is no longer needed. + * Failing to do so will result in leaked memory. + * + * \see rcl_get_publisher_names_and_types_by_node for details on the `no_demangle` parameter. + * + * The returned names are not automatically remapped by this function. + * Attempting to create publishers or subscribers using names returned by this function may not + * result in the desired topic name being used depending on the remap rules in use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator allocator to be used when allocating space for strings + * \param[in] no_demangle if true, list all topics without any demangling + * \param[out] topic_names_and_types list of topic names and their types + * \return #RCL_RET_OK if the query was successful, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID_NAME if the node name is invalid, or + * \return #RCL_RET_NODE_INVALID_NAMESPACE if the node namespace is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_topic_names_and_types( + const rcl_node_t * node, + rcl_allocator_t * allocator, + bool no_demangle, + rcl_names_and_types_t * topic_names_and_types); + +/// Return a list of service names and their types. +/** + * The `node` parameter must point to a valid node. + * + * The `service_names_and_types` parameter must be allocated and zero initialized. + * This function allocates memory for the returned list of names and types and so it is the + * callers responsibility to pass `service_names_and_types` to rcl_names_and_types_fini() + * when it is no longer needed. + * Failing to do so will result in leaked memory. + * + * The returned names are not automatically remapped by this function. + * Attempting to create clients or services using names returned by this function may not result in + * the desired service name being used depending on the remap rules in use. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator allocator to be used when allocating space for strings + * \param[out] service_names_and_types list of service names and their types + * \return #RCL_RET_OK if the query was successful, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_service_names_and_types( + const rcl_node_t * node, + rcl_allocator_t * allocator, + rcl_names_and_types_t * service_names_and_types); + +/// Initialize a rcl_names_and_types_t object. +/** + * This function initializes the string array for the names and allocates space + * for all the string arrays for the types according to the given size, but + * it does not initialize the string array for each set of types. + * However, the string arrays for each set of types is zero initialized. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] names_and_types object to be initialized + * \param[in] size the number of names and sets of types to be stored + * \param[in] allocator to be used to allocate and deallocate memory + * \return #RCL_RET_OK on success, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if memory allocation fails, or + * \return #RCL_RET_ERROR when an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_names_and_types_init( + rcl_names_and_types_t * names_and_types, + size_t size, + rcl_allocator_t * allocator); + +/// Finalize a rcl_names_and_types_t object. +/** + * The object is populated when given to one of the rcl_get_*_names_and_types() + * functions. + * This function reclaims any resources allocated during population. + * + * The `names_and_types` parameter must not be `NULL`, and must point to an + * already allocated rcl_names_and_types_t struct that was previously + * passed to a successful rcl_get_*_names_and_types() function call. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] names_and_types struct to be finalized + * \return #RCL_RET_OK if successful, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_names_and_types_fini(rcl_names_and_types_t * names_and_types); + +/// Return a list of available nodes in the ROS graph. +/** + * The `node` parameter must point to a valid node. + * + * The `node_names` parameter must be allocated and zero initialized. + * `node_names` is the output for this function, and contains allocated memory. + * Use rcutils_get_zero_initialized_string_array() for initializing an empty + * rcutils_string_array_t struct. + * This `node_names` struct should therefore be passed to rcutils_string_array_fini() + * when it is no longer needed. + * Failing to do so will result in leaked memory. + * + * Example: + * + * ```c + * rcutils_string_array_t node_names = + * rcutils_get_zero_initialized_string_array(); + * rcl_ret_t ret = rcl_get_node_names(node, &node_names); + * if (ret != RCL_RET_OK) { + * // ... error handling + * } + * // ... use the node_names struct, and when done: + * rcutils_ret_t rcutils_ret = rcutils_string_array_fini(&node_names); + * if (rcutils_ret != RCUTILS_RET_OK) { + * // ... error handling + * } + * ``` + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator used to control allocation and deallocation of names + * \param[out] node_names struct storing discovered node names + * \param[out] node_namespaces struct storing discovered node namespaces + * \return #RCL_RET_OK if the query was successful, or + * \return #RCL_RET_BAD_ALLOC if an error occurred while allocating memory, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_NODE_INVALID_NAME if a node with an invalid name is detected, or + * \return #RCL_RET_NODE_INVALID_NAMESPACE if a node with an invalid namespace is detected, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_node_names( + const rcl_node_t * node, + rcl_allocator_t allocator, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces); + +/// Return a list of available nodes in the ROS graph, including their enclave names. +/** + * An rcl_get_node_names() equivalent, but including in its output the enclave + * name the node is using. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] RMW implementation in use may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator used to control allocation and deallocation of names + * \param[out] node_names struct storing discovered node names + * \param[out] node_namespaces struct storing discovered node namespaces + * \param[out] enclaves struct storing discovered node enclaves + * \return #RCL_RET_OK if the query was successful, or + * \return #RCL_RET_BAD_ALLOC if an error occurred while allocating memory, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_node_names_with_enclaves( + const rcl_node_t * node, + rcl_allocator_t allocator, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * enclaves); + +/// Return the number of publishers on a given topic. +/** + * The `node` parameter must point to a valid node. + * + * The `topic_name` parameter must not be `NULL`, and must not be an empty string. + * It should also follow the topic name rules. + * \todo TODO(wjwwood): link to the topic name rules. + * + * The `count` parameter must point to a valid bool. + * The `count` parameter is the output for this function and will be set. + * + * In the event that error handling needs to allocate memory, this function + * will try to use the node's allocator. + * + * The topic name is not automatically remapped by this function. + * If there is a publisher created with topic name `foo` and remap rule `foo:=bar` then calling + * this with `topic_name` set to `bar` will return a count of 1, and with `topic_name` set to `foo` + * will return a count of 0. + * /sa rcl_remap_topic_name() + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] topic_name the name of the topic in question + * \param[out] count number of publishers on the given topic + * \return #RCL_RET_OK if the query was successful, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_count_publishers( + const rcl_node_t * node, + const char * topic_name, + size_t * count); + +/// Return the number of subscriptions on a given topic. +/** + * The `node` parameter must point to a valid node. + * + * The `topic_name` parameter must not be `NULL`, and must not be an empty string. + * It should also follow the topic name rules. + * \todo TODO(wjwwood): link to the topic name rules. + * + * The `count` parameter must point to a valid bool. + * The `count` parameter is the output for this function and will be set. + * + * In the event that error handling needs to allocate memory, this function + * will try to use the node's allocator. + * + * The topic name is not automatically remapped by this function. + * If there is a subscriber created with topic name `foo` and remap rule `foo:=bar` then calling + * this with `topic_name` set to `bar` will return a count of 1, and with `topic_name` set to `foo` + * will return a count of 0. + * /sa rcl_remap_topic_name() + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] topic_name the name of the topic in question + * \param[out] count number of subscriptions on the given topic + * \return #RCL_RET_OK if the query was successful, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_count_subscribers( + const rcl_node_t * node, + const char * topic_name, + size_t * count); + +/// Wait for there to be a specified number of publishers on a given topic. +/** + * The `node` parameter must point to a valid node. + * The nodes graph guard condition is used by this function, and therefore the caller should + * take care not to use the guard condition concurrently in any other wait sets. + * + * The `allocator` parameter must point to a valid allocator. + * + * The `topic_name` parameter must not be `NULL`, and must not be an empty string. + * It should also follow the topic name rules. + * + * This function blocks and will return when the number of publishers for `topic_name` + * is greater than or equal to the `count` parameter, or the specified `timeout` is reached. + * + * The `timeout` parameter is in nanoseconds. + * The timeout is based on system time elapsed. + * A negative value disables the timeout (i.e. this function blocks until the number of + * publishers is greater than or equals to `count`). + * + * The `success` parameter must point to a valid bool. + * The `success` parameter is the output for this function and will be set. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator to allocate space for the rcl_wait_set_t used to wait for graph events + * \param[in] topic_name the name of the topic in question + * \param[in] count number of publishers to wait for + * \param[in] timeout maximum duration to wait for publishers + * \param[out] success `true` if the number of publishers is equal to or greater than count, or + * `false` if a timeout occurred waiting for publishers. + * \return #RCL_RET_OK if there was no errors, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMEOUT if a timeout occurs before the number of publishers is detected, or + * \return #RCL_RET_ERROR if an unspecified error occurred. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_for_publishers( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * topic_name, + const size_t count, + rcutils_duration_value_t timeout, + bool * success); + +/// Wait for there to be a specified number of subscribers on a given topic. +/** + * \see rcl_wait_for_publishers + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator to allocate space for the rcl_wait_set_t used to wait for graph events + * \param[in] topic_name the name of the topic in question + * \param[in] count number of subscribers to wait for + * \param[in] timeout maximum duration to wait for subscribers + * \param[out] success `true` if the number of subscribers is equal to or greater than count, or + * `false` if a timeout occurred waiting for subscribers. + * \return #RCL_RET_OK if there was no errors, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMEOUT if a timeout occurs before the number of subscribers is detected, or + * \return #RCL_RET_ERROR if an unspecified error occurred. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_for_subscribers( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * topic_name, + const size_t count, + rcutils_duration_value_t timeout, + bool * success); + +/// Return a list of all publishers to a topic. +/** + * The `node` parameter must point to a valid node. + * + * The `topic_name` parameter must not be `NULL`. + * + * When the `no_mangle` parameter is `true`, the provided `topic_name` should be a valid topic name + * for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps). + * When the `no_mangle` parameter is `false`, the provided `topic_name` should follow + * ROS topic name conventions. + * In either case, the topic name should always be fully qualified. + * + * Each element in the `publishers_info` array will contain the node name, node namespace, + * topic type, gid and the qos profile of the publisher. + * It is the responsibility of the caller to ensure that `publishers_info` parameter points + * to a valid struct of type rcl_topic_endpoint_info_array_t. + * The `count` field inside the struct must be set to 0 and the `info_array` field inside + * the struct must be set to null. + * \see rmw_get_zero_initialized_topic_endpoint_info_array + * + * The `allocator` will be used to allocate memory to the `info_array` member + * inside of `publishers_info`. + * Moreover, every const char * member inside of + * rmw_topic_endpoint_info_t will be assigned a copied value on allocated memory. + * \see rmw_topic_endpoint_info_set_node_name and the likes. + * However, it is the responsibility of the caller to + * reclaim any allocated resources to `publishers_info` to avoid leaking memory. + * \see rmw_topic_endpoint_info_array_fini + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator allocator to be used when allocating space for + * the array inside publishers_info + * \param[in] topic_name the name of the topic in question + * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name, + * otherwise it should be a valid ROS topic name + * \param[out] publishers_info a struct representing a list of publisher information + * \return #RCL_RET_OK if the query was successful, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if memory allocation fails, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_publishers_info_by_topic( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rcl_topic_endpoint_info_array_t * publishers_info); + +/// Return a list of all subscriptions to a topic. +/** + * The `node` parameter must point to a valid node. + * + * The `topic_name` parameter must not be `NULL`. + * + * When the `no_mangle` parameter is `true`, the provided `topic_name` should be a valid topic name + * for the middleware (useful when combining ROS with native middleware (e.g. DDS) apps). + * When the `no_mangle` parameter is `false`, the provided `topic_name` should follow + * ROS topic name conventions. + * In either case, the topic name should always be fully qualified. + * + * Each element in the `subscriptions_info` array will contain the node name, node namespace, + * topic type, gid and the qos profile of the subscription. + * It is the responsibility of the caller to ensure that `subscriptions_info` parameter points + * to a valid struct of type rcl_topic_endpoint_info_array_t. + * The `count` field inside the struct must be set to 0 and the `info_array` field inside + * the struct must be set to null. + * \see rmw_get_zero_initialized_topic_endpoint_info_array + * + * The `allocator` will be used to allocate memory to the `info_array` member + * inside of `subscriptions_info`. + * Moreover, every const char * member inside of + * rmw_topic_endpoint_info_t will be assigned a copied value on allocated memory. + * \see rmw_topic_endpoint_info_set_node_name and the likes. + * However, it is the responsibility of the caller to + * reclaim any allocated resources to `subscriptions_info` to avoid leaking memory. + * \see rmw_topic_endpoint_info_array_fini + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] allocator allocator to be used when allocating space for + * the array inside publishers_info + * \param[in] topic_name the name of the topic in question + * \param[in] no_mangle if `true`, `topic_name` needs to be a valid middleware topic name, + * otherwise it should be a valid ROS topic name + * \param[out] subscriptions_info a struct representing a list of subscriptions information + * \return #RCL_RET_OK if the query was successful, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if memory allocation fails, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_get_subscriptions_info_by_topic( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rcl_topic_endpoint_info_array_t * subscriptions_info); + +/// Check if a service server is available for the given service client. +/** + * This function will return true for `is_available` if there is a service server + * available for the given client. + * + * The `node` parameter must point to a valid node. + * + * The `client` parameter must point to a valid client. + * + * The given client and node must match, i.e. the client must have been created + * using the given node. + * + * The `is_available` parameter must not be `NULL`, and must point a bool variable. + * The result of the check will be stored in the `is_available` parameter. + * + * In the event that error handling needs to allocate memory, this function + * will try to use the node's allocator. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] node the handle to the node being used to query the ROS graph + * \param[in] client the handle to the service client being queried + * \param[out] is_available set to true if there is a service server available, else false + * \return #RCL_RET_OK if the check was made successfully (regardless of the service readiness), or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_server_is_available( + const rcl_node_t * node, + const rcl_client_t * client, + bool * is_available); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__GRAPH_H_ diff --git a/include/rcl/guard_condition.h b/include/rcl/guard_condition.h new file mode 100644 index 0000000..368a9a6 --- /dev/null +++ b/include/rcl/guard_condition.h @@ -0,0 +1,269 @@ +// Copyright 2015 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. + +/// @file + +#ifndef RCL__GUARD_CONDITION_H_ +#define RCL__GUARD_CONDITION_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/allocator.h" +#include "rcl/context.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +/// Internal rcl guard condition implementation struct. +typedef struct rcl_guard_condition_impl_s rcl_guard_condition_impl_t; + +/// Handle for a rcl guard condition. +typedef struct rcl_guard_condition_s +{ + /// Context associated with this guard condition. + rcl_context_t * context; + + /// Pointer to the guard condition implementation + rcl_guard_condition_impl_t * impl; +} rcl_guard_condition_t; + +/// Options available for a rcl guard condition. +typedef struct rcl_guard_condition_options_s +{ + /// Custom allocator for the guard condition, used for internal allocations. + rcl_allocator_t allocator; +} rcl_guard_condition_options_t; + +/// Return a rcl_guard_condition_t struct with members set to `NULL`. +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_guard_condition_t +rcl_get_zero_initialized_guard_condition(void); + +/// Initialize a rcl guard_condition. +/** + * After calling this function on a rcl_guard_condition_t, it can be passed to + * rcl_wait() and then concurrently it can be triggered to wake-up rcl_wait(). + * + * Expected usage: + * + * ```c + * #include + * + * // ... error handling + * rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition(); + * // ... customize guard condition options + * rcl_ret_t ret = rcl_guard_condition_init( + * &guard_condition, context, rcl_guard_condition_get_default_options()); + * // ... error handling, and on shutdown do deinitialization: + * ret = rcl_guard_condition_fini(&guard_condition); + * // ... error handling for rcl_guard_condition_fini() + * ``` + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] guard_condition preallocated guard_condition structure + * \param[in] context the context instance with which the guard condition + * should be associated + * \param[in] options the guard_condition's options + * \return #RCL_RET_OK if guard_condition was initialized successfully, or + * \return #RCL_RET_ALREADY_INIT if the guard condition is already initialized, or + * \return #RCL_RET_NOT_INIT if the given context is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_guard_condition_init( + rcl_guard_condition_t * guard_condition, + rcl_context_t * context, + const rcl_guard_condition_options_t options); + +/// Same as rcl_guard_condition_init(), but reusing an existing rmw handle. +/** + * In addition to the documentation for rcl_guard_condition_init(), the + * `rmw_guard_condition` parameter must not be `NULL` and must point to a valid + * rmw guard condition. + * + * Also the life time of the rcl guard condition is tied to the life time of + * the rmw guard condition. + * So if the rmw guard condition is destroyed before the rcl guard condition, + * the rcl guard condition becomes invalid. + * + * Similarly if the resulting rcl guard condition is fini'ed before the rmw + * guard condition, then the rmw guard condition is no longer valid. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] guard_condition preallocated guard_condition structure + * \param[in] rmw_guard_condition existing rmw guard condition to reuse + * \param[in] context the context instance with which the rmw guard condition + * was initialized with, i.e. the rmw context inside rcl context needs to + * match rmw context in rmw guard condition + * \param[in] options the guard_condition's options + * \return #RCL_RET_OK if guard_condition was initialized successfully, or + * \return #RCL_RET_ALREADY_INIT if the guard condition is already initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +rcl_ret_t +rcl_guard_condition_init_from_rmw( + rcl_guard_condition_t * guard_condition, + const rmw_guard_condition_t * rmw_guard_condition, + rcl_context_t * context, + const rcl_guard_condition_options_t options); + +/// Finalize a rcl_guard_condition_t. +/** + * After calling, calls to rcl_trigger_guard_condition() will fail when using + * this guard condition. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * [1] specifically not thread-safe with rcl_trigger_guard_condition() + * + * \param[inout] guard_condition handle to the guard_condition to be finalized + * \return #RCL_RET_OK if guard_condition was finalized successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition); + +/// Return the default options in a rcl_guard_condition_options_t struct. +/** + * The defaults are: + * + * - allocator = rcl_get_default_allocator() + * + * \return the default options in an rcl_guard_condition_options_t struct. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_guard_condition_options_t +rcl_guard_condition_get_default_options(void); + +/// Trigger a rcl guard condition. +/** + * This function can fail, and return RCL_RET_INVALID_ARGUMENT, if the: + * - guard condition is `NULL` + * - guard condition is invalid (never called init or called fini) + * + * A guard condition can be triggered from any thread. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * [1] it can be called concurrently with itself, even on the same guard condition + * + * \param[in] guard_condition handle to the guard_condition to be triggered + * \return #RCL_RET_OK if the guard condition was triggered, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_trigger_guard_condition(rcl_guard_condition_t * guard_condition); + +/// Return the guard condition options. +/** + * Returned is a pointer to the internally held rcl_guard_condition_options_t. + * This function can fail, and therefore return `NULL`, if the: + * - guard_condition is `NULL` + * - guard_condition is invalid (never called init, called fini, or invalid node) + * + * The returned pointer is made invalid if the guard condition is finalized. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] guard_condition pointer to the rcl guard_condition + * \return rcl guard condition options if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rcl_guard_condition_options_t * +rcl_guard_condition_get_options(const rcl_guard_condition_t * guard_condition); + +/// Return the rmw guard condition handle. +/** + * The handle returned is a pointer to the internally held rmw handle. + * This function can fail, and therefore return `NULL`, if the: + * - guard_condition is `NULL` + * - guard_condition is invalid (never called init, called fini, or invalid node) + * + * The returned handle is made invalid if the guard condition is finalized or + * if rcl_shutdown() is called. + * The returned handle is not guaranteed to be valid for the life time of the + * guard condition as it may be finalized and recreated itself. + * Therefore it is recommended to get the handle from the guard condition using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] guard_condition pointer to the rcl guard_condition + * \return rmw guard condition handle if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_guard_condition_t * +rcl_guard_condition_get_rmw_handle(const rcl_guard_condition_t * guard_condition); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__GUARD_CONDITION_H_ diff --git a/include/rcl/init.h b/include/rcl/init.h new file mode 100644 index 0000000..a85aae6 --- /dev/null +++ b/include/rcl/init.h @@ -0,0 +1,121 @@ +// Copyright 2014 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. + +/// @file + +#ifndef RCL__INIT_H_ +#define RCL__INIT_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/allocator.h" +#include "rcl/context.h" +#include "rcl/init_options.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +/// Initialization of rcl. +/** + * This function can be run any number of times, so long as the given context + * has been properly prepared. + * + * The given rcl_context_t must be zero initialized with the function + * rcl_get_zero_initialized_context() and must not be already initialized + * by this function. + * If the context is already initialized this function will fail and return the + * #RCL_RET_ALREADY_INIT error code. + * A context may be initialized again after it has been finalized with the + * rcl_shutdown() function and zero initialized again with + * rcl_get_zero_initialized_context(). + * + * The `argc` and `argv` parameters may contain command line arguments for the + * program. + * rcl specific arguments will be parsed, but not removed. + * If `argc` is `0` and `argv` is `NULL` no parameters will be parsed. + * + * The `options` argument must be non-`NULL` and must have been initialized + * with rcl_init_options_init(). + * It is unmodified by this function, and the ownership is not transfered to + * the context, but instead a copy is made into the context for later reference. + * Therefore, the given options need to be cleaned up with + * rcl_init_options_fini() after this function returns. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_uint_least64_t` + * + * \param[in] argc number of strings in argv + * \param[in] argv command line arguments; rcl specific arguments are removed + * \param[in] options options used during initialization + * \param[out] context resulting context object that represents this init + * \return #RCL_RET_OK if initialization is successful, or + * \return #RCL_RET_ALREADY_INIT if rcl_init has already been called, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_INVALID_ROS_ARGS if an invalid ROS argument is found, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_init( + int argc, + char const * const * argv, + const rcl_init_options_t * options, + rcl_context_t * context); + +/// Shutdown a given rcl context. +/** + * The given context must have been initialized with rcl_init(). + * If not, this function will fail with #RCL_RET_ALREADY_SHUTDOWN. + * + * When this function is called: + * - Any rcl objects created using this context are invalidated. + * - Functions called on invalid objects may or may not fail. + * - Calls to rcl_context_is_initialized() will return `false`. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_uint_least64_t` + * + * \param[inout] context object to shutdown + * \return #RCL_RET_OK if the shutdown was completed successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ALREADY_SHUTDOWN if the context is not currently valid, or + * \return #RCL_RET_ERROR if an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_shutdown(rcl_context_t * context); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__INIT_H_ diff --git a/include/rcl/init_options.h b/include/rcl/init_options.h new file mode 100644 index 0000000..f80dd77 --- /dev/null +++ b/include/rcl/init_options.h @@ -0,0 +1,230 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file + +#ifndef RCL__INIT_OPTIONS_H_ +#define RCL__INIT_OPTIONS_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rmw/init.h" + +#include "rcl/allocator.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +typedef struct rcl_init_options_impl_s rcl_init_options_impl_t; + +/// Encapsulation of init options and implementation defined init options. +typedef struct rcl_init_options_s +{ + /// Implementation specific pointer. + rcl_init_options_impl_t * impl; +} rcl_init_options_t; + +/// Return a zero initialized rcl_init_options_t struct. +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_init_options_t +rcl_get_zero_initialized_init_options(void); + +/// Initialize given init_options with the default values and implementation specific values. +/** + * The given allocator is used, if required, during setup of the init options, + * but is also used during initialization. + * + * In either case the given allocator is stored in the returned init options. + * + * The `impl` pointer should not be changed manually. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Yes + * Lock-Free | Yes + * + * \param[inout] init_options object to be setup + * \param[in] allocator to be used during setup and during initialization + * \return #RCL_RET_OK if setup is successful, or + * \return #RCL_RET_ALREADY_INIT if init_options has already be initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_init_options_init(rcl_init_options_t * init_options, rcl_allocator_t allocator); + +/// Copy the given source init_options to the destination init_options. +/** + * The allocator from the source is used for any allocations and stored in the + * destination. + * + * The destination should either be zero initialized with + * rcl_get_zero_initialized_init_options() or should have had + * rcl_init_options_fini() called on it. + * Giving an already initialized init options for the destination will result + * in a failure with return code #RCL_RET_ALREADY_INIT. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Yes + * Lock-Free | Yes + * + * \param[in] src rcl_init_options_t object to be copied from + * \param[out] dst rcl_init_options_t object to be copied into + * \return #RCL_RET_OK if the copy is successful, or + * \return #RCL_RET_ALREADY_INIT if the dst has already be initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_init_options_copy(const rcl_init_options_t * src, rcl_init_options_t * dst); + +/// Finalize the given init_options. +/** + * The given init_options must be non-`NULL` and valid, i.e. had + * rcl_init_options_init() called on it but not this function yet. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Yes + * Lock-Free | Yes + * + * \param[inout] init_options object to be setup + * \return #RCL_RET_OK if setup is successful, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_init_options_fini(rcl_init_options_t * init_options); + +/// Return the domain_id stored in the init options. +/** + * Get the domain id from the specified rcl_init_options_t object. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] init_options object from which the domain id should be retrieved. + * \param[out] domain_id domain id to be set in init_options object. + * \return #RCL_RET_OK if successful, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_init_options_get_domain_id(const rcl_init_options_t * init_options, size_t * domain_id); + +/// Set a domain id in the init options provided. +/** + * Store the domain id in the specified init_options object. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] init_options objects in which to set the specified domain id. + * \param[in] domain_id domain id to be set in init_options object. + * \return #RCL_RET_OK if successful, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_init_options_set_domain_id(rcl_init_options_t * init_options, size_t domain_id); + +/// Return the rmw init options which are stored internally. +/** + * This function can fail and return `NULL` if: + * - init_options is NULL + * - init_options is invalid, e.g. init_options->impl is NULL + * + * If NULL is returned an error message will have been set. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | Yes + * Lock-Free | Yes + * + * \param[in] init_options object from which the rmw init options should be retrieved + * \return pointer to the the rcl init options, or + * \return `NULL` if there was an error + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_init_options_t * +rcl_init_options_get_rmw_init_options(rcl_init_options_t * init_options); + +/// Return the allocator stored in the init_options. +/** + * This function can fail and return `NULL` if: + * - init_options is NULL + * - init_options is invalid, e.g. init_options->impl is NULL + * + * If NULL is returned an error message will have been set. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] init_options object from which the allocator should be retrieved + * \return pointer to the rcl allocator, or + * \return `NULL` if there was an error + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rcl_allocator_t * +rcl_init_options_get_allocator(const rcl_init_options_t * init_options); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__INIT_OPTIONS_H_ diff --git a/include/rcl/lexer.h b/include/rcl/lexer.h new file mode 100644 index 0000000..7cf3dac --- /dev/null +++ b/include/rcl/lexer.h @@ -0,0 +1,120 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file + +#ifndef RCL__LEXER_H_ +#define RCL__LEXER_H_ + +#include + +#include "rcl/allocator.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +#if __cplusplus +extern "C" +{ +#endif + +/// Type of lexeme found by lexical analysis. +typedef enum rcl_lexeme_e +{ + /// Indicates no valid lexeme was found (end of input not reached) + RCL_LEXEME_NONE = 0, + /// Indicates end of input has been reached + RCL_LEXEME_EOF = 1, + /// ~/ + RCL_LEXEME_TILDE_SLASH = 2, + /// rosservice:// + RCL_LEXEME_URL_SERVICE = 3, + /// rostopic:// + RCL_LEXEME_URL_TOPIC = 4, + /// : + RCL_LEXEME_COLON = 5, + /// __node or __name + RCL_LEXEME_NODE = 6, + /// __ns + RCL_LEXEME_NS = 7, + /// := + RCL_LEXEME_SEPARATOR = 8, + /// \1 + RCL_LEXEME_BR1 = 9, + /// \2 + RCL_LEXEME_BR2 = 10, + /// \3 + RCL_LEXEME_BR3 = 11, + /// \4 + RCL_LEXEME_BR4 = 12, + /// \5 + RCL_LEXEME_BR5 = 13, + /// \6 + RCL_LEXEME_BR6 = 14, + /// \7 + RCL_LEXEME_BR7 = 15, + /// \8 + RCL_LEXEME_BR8 = 16, + /// \9 + RCL_LEXEME_BR9 = 17, + /// a name between slashes, must match (([a-zA-Z](_)?)|_)([0-9a-zA-Z](_)?)* + RCL_LEXEME_TOKEN = 18, + /// / + RCL_LEXEME_FORWARD_SLASH = 19, + /// * + RCL_LEXEME_WILD_ONE = 20, + /// ** + RCL_LEXEME_WILD_MULTI = 21, + // TODO(hidmic): remove when parameter names are standardized to + // use slashes in lieu of dots + /// \. + RCL_LEXEME_DOT = 22, +} rcl_lexeme_t; + + +/// Do lexical analysis on a string. +/** + * This function analyzes a string to see if it starts with a valid lexeme. + * If the string does not begin with a valid lexeme then lexeme will be RCL_LEXEME_NONE, and the + * length will be set to include the character that made it impossible. + * If the first character is '\0' then lexeme will be RCL_LEXEME_EOF. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] text The string to analyze. + * \param[out] lexeme The type of lexeme found in the string. + * \param[out] length The length of text in the string that constitutes the found lexeme. + * \return #RCL_RET_OK if analysis is successful regardless whether a valid lexeme is found, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_ERROR if an internal bug is detected. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_lexer_analyze( + const char * text, + rcl_lexeme_t * lexeme, + size_t * length); + +#if __cplusplus +} +#endif + +#endif // RCL__LEXER_H_ diff --git a/include/rcl/lexer_lookahead.h b/include/rcl/lexer_lookahead.h new file mode 100644 index 0000000..2bf523c --- /dev/null +++ b/include/rcl/lexer_lookahead.h @@ -0,0 +1,264 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file + +#ifndef RCL__LEXER_LOOKAHEAD_H_ +#define RCL__LEXER_LOOKAHEAD_H_ + +#include + +#include "rcl/allocator.h" +#include "rcl/lexer.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +#if __cplusplus +extern "C" +{ +#endif + +// Forward declaration +typedef struct rcl_lexer_lookahead2_impl_s rcl_lexer_lookahead2_impl_t; + +/// Track lexical analysis and allow looking ahead 2 lexemes. +typedef struct rcl_lexer_lookahead2_s +{ + /// Pointer to the lexer look ahead2 implementation + rcl_lexer_lookahead2_impl_t * impl; +} rcl_lexer_lookahead2_t; + +/// Get a zero initialized rcl_lexer_lookahead2_t instance. +/** + * \sa rcl_lexer_lookahead2_init() + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \return zero initialized lookahead2 buffer. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_lexer_lookahead2_t +rcl_get_zero_initialized_lexer_lookahead2(); + +/// Initialize an rcl_lexer_lookahead2_t instance. +/** + * The lookahead2 buffer borrows a reference to the provided text. + * The text must not be freed before the buffer is finalized. + * The lookahead2 buffer only needs to be finalized if this function does not return RCL_RET_OK. + * \sa rcl_lexer_lookahead2_fini() + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] buffer A buffer that is zero initialized. + * \sa rcl_get_zero_initialized_lexer_lookahead2() + * \param[in] text The string to analyze. + * \param[in] allocator An allocator to use if an error occurs. + * \return #RCL_RET_OK if the buffer is successfully initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurrs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_lexer_lookahead2_init( + rcl_lexer_lookahead2_t * buffer, + const char * text, + rcl_allocator_t allocator); + +/// Finalize an instance of an rcl_lexer_lookahead2_t structure. +/** + * \sa rcl_lexer_lookahead2_init() + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes [1] + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * [1] Only allocates if an argument is invalid. + * + * \param[in] buffer The structure to be deallocated. + * \return #RCL_RET_OK if the structure was successfully finalized, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_lexer_lookahead2_fini( + rcl_lexer_lookahead2_t * buffer); + +/// Look ahead at the next lexeme in the string. +/** + * Repeated calls to peek will return the same lexeme. + * A parser that deems the next lexeme as valid must accept it to advance lexing. + * \sa rcl_lexer_lookahead2_accept() + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes [1] + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * [1] Only allocates if an argument is invalid or an internal bug is detected. + * + * \param[in] buffer the lookahead2 buffer being used to analyze a string. + * \param[out] next_type an output variable for the next lexeme in the string. + * \return #RCL_RET_OK if peeking was successfull, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_lexer_lookahead2_peek( + rcl_lexer_lookahead2_t * buffer, + rcl_lexeme_t * next_type); + +/// Look ahead at the next two lexemes in the string. +/** + * Repeated calls to peek2 will return the same two lexemes. + * A parser that deems the next two lexemes as valid must accept twice to advance lexing. + * \sa rcl_lexer_lookahead2_accept() + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes [1] + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * [1] Only allocates if an argument is invalid or an internal bug is detected. + * + * \param[in] buffer the lookahead2 buffer being used to analyze a string. + * \param[out] next_type1 an output variable for the next lexeme in the string. + * \param[out] next_type2 an output variable for the lexeme after the next lexeme in the string. + * \return #RCL_RET_OK if peeking was successfull, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_lexer_lookahead2_peek2( + rcl_lexer_lookahead2_t * buffer, + rcl_lexeme_t * next_type1, + rcl_lexeme_t * next_type2); + +/// Accept a lexeme and advance analysis. +/** + * A token must have been peeked before it can be accepted. + * \sa rcl_lexer_lookahead2_peek() + * \sa rcl_lexer_lookahead2_peek2() + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes [1] + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * [1] Only allocates if an argument is invalid or an error occurs. + * + * \param[in] buffer the lookahead2 buffer being used to analyze a string. + * \param[out] lexeme_text pointer to where lexeme begins in string. + * \param[out] lexeme_text_length length of lexeme_text. + * \return #RCL_RET_OK if peeking was successfull, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_lexer_lookahead2_accept( + rcl_lexer_lookahead2_t * buffer, + const char ** lexeme_text, + size_t * lexeme_text_length); + +/// Require the next lexeme to be a certain type and advance analysis. +/** + * This method is a shortcut to peeking and accepting a lexeme. + * It should be used by a parser when there is only one valid lexeme that could come next. + * \sa rcl_lexer_lookahead2_peek() + * \sa rcl_lexer_lookahead2_accept() + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes [1] + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * [1] Only allocates if an argument is invalid or an error occurs. + * + * \param[in] buffer the lookahead2 buffer being used to analyze a string. + * \param[in] type the type the next lexeme must be. + * \param[out] lexeme_text pointer to where lexeme begins in string. + * \param[out] lexeme_text_length length of lexeme_text. + * \return #RCL_RET_OK if the next lexeme was the expected one, or + * \return #RCL_RET_WRONG_LEXEME if the next lexeme was not the expected one, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_lexer_lookahead2_expect( + rcl_lexer_lookahead2_t * buffer, + rcl_lexeme_t type, + const char ** lexeme_text, + size_t * lexeme_text_length); + +/// Get the text at the point where it is currently being analyzed. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] buffer the lookahead2 buffer being used to analyze a string. + * \return a pointer inside the original text at the position being analyzed, or + * \return `NULL` if buffer is itself `NULL` or zero initialized, or + * \return an undefined value if buffer is not initialized or has been finalized. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_lexer_lookahead2_get_text( + const rcl_lexer_lookahead2_t * buffer); + +#if __cplusplus +} +#endif + +#endif // RCL__LEXER_LOOKAHEAD_H_ diff --git a/include/rcl/localhost.h b/include/rcl/localhost.h new file mode 100644 index 0000000..2c5123a --- /dev/null +++ b/include/rcl/localhost.h @@ -0,0 +1,48 @@ +// 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. + +/// @file + +#ifndef RCL__LOCALHOST_H_ +#define RCL__LOCALHOST_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/types.h" +#include "rcl/visibility_control.h" +#include "rmw/localhost.h" + +extern const char * const RCL_LOCALHOST_ENV_VAR; + +/// Determine if the user wants to communicate using loopback only. +/** + * Checks if localhost should be used for network communication based on environment. + * + * \param[out] localhost_only Must not be NULL. + * \return #RCL_RET_INVALID_ARGUMENT if an argument is invalid, or + * \return #RCL_RET_ERROR if an unexpected error happened, or + * \return #RCL_RET_OK. + */ +RCL_PUBLIC +rcl_ret_t +rcl_get_localhost_only(rmw_localhost_only_t * localhost_only); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__LOCALHOST_H_ diff --git a/include/rcl/log_level.h b/include/rcl/log_level.h new file mode 100644 index 0000000..64942ec --- /dev/null +++ b/include/rcl/log_level.h @@ -0,0 +1,193 @@ +// 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. + +/// @file + +#ifndef RCL__LOG_LEVEL_H_ +#define RCL__LOG_LEVEL_H_ + +#include "rcl/allocator.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// typedef for RCUTILS_LOG_SEVERITY; +typedef enum RCUTILS_LOG_SEVERITY rcl_log_severity_t; + +/// A logger item to specify a name and a log level. +typedef struct rcl_logger_setting_s +{ + /// Name for the logger. + const char * name; + /// Minimum log level severity of the logger. + rcl_log_severity_t level; +} rcl_logger_setting_t; + +/// Hold default logger level and other logger setting. +typedef struct rcl_log_levels_s +{ + /// Minimum default logger level severity. + rcl_log_severity_t default_logger_level; + /// Array of logger setting. + rcl_logger_setting_t * logger_settings; + /// Number of logger settings. + size_t num_logger_settings; + /// Capacity of logger settings. + size_t capacity_logger_settings; + /// Allocator used to allocate objects in this struct. + rcl_allocator_t allocator; +} rcl_log_levels_t; + +/// Return a rcl_log_levels_t struct with members initialized to zero value. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \return a rcl_log_levels_t struct with members initialized to zero value. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_log_levels_t +rcl_get_zero_initialized_log_levels(); + +/// Initialize a log levels structure. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] log_levels The structure to be initialized. + * \param[in] allocator Memory allocator to be used and assigned into log_levels. + * \param[in] logger_count Number of logger settings to be allocated. + * This reserves memory for logger_settings, but doesn't initialize it. + * \return #RCL_RET_OK if the structure was initialized successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if log_levels is NULL, or + * \return #RCL_RET_INVALID_ARGUMENT if log_levels contains initialized memory, or + * \return #RCL_RET_INVALID_ARGUMENT if allocator is invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_log_levels_init( + rcl_log_levels_t * log_levels, const rcl_allocator_t * allocator, size_t logger_count); + +/// Copy one log levels structure into another. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] src The structure to be copied. + * Its allocator is used to copy memory into the new structure. + * \param[out] dst A log levels structure to be copied into. + * \return #RCL_RET_OK if the structure was copied successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if src is NULL, or + * \return #RCL_RET_INVALID_ARGUMENT if src allocator is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if dst is NULL, or + * \return #RCL_RET_INVALID_ARGUMENT if dst contains already allocated memory, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_log_levels_copy(const rcl_log_levels_t * src, rcl_log_levels_t * dst); + +/// Reclaim resources held inside rcl_log_levels_t structure. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] log_levels The structure which its resources have to be deallocated. + * \return #RCL_RET_OK if the memory was successfully freed, or + * \return #RCL_RET_INVALID_ARGUMENT if log_levels is NULL, or + * \return #RCL_RET_INVALID_ARGUMENT if the log_levels allocator is invalid and the structure contains initialized memory. + */ +RCL_PUBLIC +rcl_ret_t +rcl_log_levels_fini(rcl_log_levels_t * log_levels); + +/// Shrink log levels structure. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] log_levels The structure to be shrunk. + * \return #RCL_RET_OK if the memory was successfully shrunk, or + * \return #RCL_RET_INVALID_ARGUMENT if log_levels is NULL or if its allocator is invalid, or + * \return #RCL_RET_BAD_ALLOC if reallocating memory failed. + */ +RCL_PUBLIC +rcl_ret_t +rcl_log_levels_shrink_to_size(rcl_log_levels_t * log_levels); + +/// Add logger setting with a name and a level. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] log_levels The structure where to set the logger log level. + * \param[in] logger_name Name for the logger, a copy of it will be stored in the structure. + * \param[in] log_level Minimum log level severity to be set for logger_name. + * \return #RCL_RET_OK if add logger setting successfully, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_INVALID_ARGUMENT if log_levels is NULL, or + * \return #RCL_RET_INVALID_ARGUMENT if log_levels was not initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if log_levels allocator is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if logger_name is NULL, or + * \return #RCL_RET_ERROR if the log_levels structure is already full. + */ +RCL_PUBLIC +rcl_ret_t +rcl_log_levels_add_logger_setting( + rcl_log_levels_t * log_levels, const char * logger_name, rcl_log_severity_t log_level); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__LOG_LEVEL_H_ diff --git a/include/rcl/logging.h b/include/rcl/logging.h new file mode 100644 index 0000000..23f54b2 --- /dev/null +++ b/include/rcl/logging.h @@ -0,0 +1,162 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file + +#ifndef RCL__LOGGING_H_ +#define RCL__LOGGING_H_ + +#include "rcl/allocator.h" +#include "rcl/arguments.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +#include "rcutils/logging.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// The function signature to log messages. +typedef rcutils_logging_output_handler_t rcl_logging_output_handler_t; + +/// Configure the logging system. +/** + * This function should be called during the ROS initialization process. + * It will add the enabled log output appenders to the root logger. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] global_args The global arguments for the system + * \param[in] allocator Used to allocate memory used by the logging system + * \return #RCL_RET_OK if successful, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if a general error occurs + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_logging_configure( + const rcl_arguments_t * global_args, + const rcl_allocator_t * allocator); + +/// Configure the logging system with the provided output handler. +/** + * Similar to rcl_logging_configure, but it uses the provided output handler. + * \sa rcl_logging_configure + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] global_args The global arguments for the system + * \param[in] allocator Used to allocate memory used by the logging system + * \param[in] output_handler Output handler to be installed + * \return #RCL_RET_OK if successful, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if a general error occurs + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_logging_configure_with_output_handler( + const rcl_arguments_t * global_args, + const rcl_allocator_t * allocator, + rcl_logging_output_handler_t output_handler); + +/** + * This function should be called to tear down the logging setup by the configure function. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \return #RCL_RET_OK if successful. + * \return #RCL_RET_ERROR if a general error occurs + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t rcl_logging_fini(void); + +/// See if logging rosout is enabled. +/** + * This function is meant to be used to check if logging rosout is enabled. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \return `TRUE` if logging rosout is enabled, or + * \return `FALSE` if logging rosout is disabled. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +bool rcl_logging_rosout_enabled(void); + +/// Default output handler used by rcl. +/** + * This function can be wrapped in a language specific client library, + * adding the necessary mutual exclusion protection there, and then use + * rcl_logging_configure_with_output_handler() instead of + * rcl_logging_configure(). + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] location The pointer to the location struct or NULL + * \param[in] severity The severity level + * \param[in] name The name of the logger, must be null terminated c string + * \param[in] timestamp The timestamp for when the log message was made + * \param[in] format The list of arguments to insert into the formatted log message + * \param[in] args argument for the string format + */ +RCL_PUBLIC +void +rcl_logging_multiple_output_handler( + const rcutils_log_location_t * location, + int severity, const char * name, rcutils_time_point_value_t timestamp, + const char * format, va_list * args); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__LOGGING_H_ diff --git a/include/rcl/logging_rosout.h b/include/rcl/logging_rosout.h new file mode 100644 index 0000000..4e42629 --- /dev/null +++ b/include/rcl/logging_rosout.h @@ -0,0 +1,195 @@ +// Copyright 2018-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. + +/// @file + +#ifndef RCL__LOGGING_ROSOUT_H_ +#define RCL__LOGGING_ROSOUT_H_ + +#include "rcl/allocator.h" +#include "rcl/error_handling.h" +#include "rcl/node.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// The default qos profile setting for topic /rosout +/** + * - depth = 1000 + * - durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL + * - lifespan = {10, 0} + */ +static const rmw_qos_profile_t rcl_qos_profile_rosout_default = +{ + RMW_QOS_POLICY_HISTORY_KEEP_LAST, + 1000, + RMW_QOS_POLICY_RELIABILITY_RELIABLE, + RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + RMW_QOS_DEADLINE_DEFAULT, + {10, 0}, + RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT, + RMW_QOS_LIVELINESS_LEASE_DURATION_DEFAULT, + false +}; + +/// Initializes the rcl_logging_rosout features +/** + * Calling this will initialize the rcl_logging_rosout features. This function must be called + * before any other rcl_logging_rosout_* functions can be called. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] allocator The allocator used for metadata related to the rcl_logging_rosout features + * \return #RCL_RET_OK if the rcl_logging_rosout features are successfully initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_logging_rosout_init( + const rcl_allocator_t * allocator); + +/// Uninitializes the rcl_logging_rosout features +/** + * Calling this will set the rcl_logging_rosout features into the an unitialized state that is + * functionally the same as before rcl_logging_rosout_init was called. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \return #RCL_RET_OK if the rcl_logging_rosout feature was successfully unitialized, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_logging_rosout_fini(); + +/// Creates a rosout publisher for a node and registers it to be used by the logging system +/** + * Calling this for an rcl_node_t will create a new publisher on that node that will be + * used by the logging system to publish all log messages from that Node's logger. + * + * If a publisher already exists for this node then a new publisher will NOT be created. + * + * It is expected that after creating a rosout publisher with this function + * rcl_logging_destroy_rosout_publisher_for_node() will be called for the node to cleanup + * the publisher while the Node is still valid. + * + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node a valid rcl_node_t that the publisher will be created on + * \return #RCL_RET_OK if the logging publisher was created successfully, or + * \return #RCL_RET_NODE_INVALID if the argument is invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_logging_rosout_init_publisher_for_node( + rcl_node_t * node); + +/// Deregisters a rosout publisher for a node and cleans up allocated resources +/** + * Calling this for an rcl_node_t will destroy the rosout publisher on that node and remove it from + * the logging system so that no more Log messages are published to this function. + * + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node a valid rcl_node_t that the publisher will be created on + * \return #RCL_RET_OK if the logging publisher was finalized successfully, or + * \return #RCL_RET_NODE_INVALID if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_logging_rosout_fini_publisher_for_node( + rcl_node_t * node); + +/// The output handler outputs log messages to rosout topics. +/** + * When called with a logger name and log message this function will attempt to + * find a rosout publisher correlated with the logger name and publish a Log + * message out via that publisher. If there is no publisher directly correlated + * with the logger then nothing will be done. + * + * This function is meant to be registered with the logging functions for + * rcutils, and shouldn't be used outside of that context. + * Additionally, arguments like args should be non-null and properly initialized + * otherwise it is undefined behavior. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] location The pointer to the location struct or NULL + * \param[in] severity The severity level + * \param[in] name The name of the logger, must be null terminated c string + * \param[in] timestamp The timestamp for when the log message was made + * \param[in] format The list of arguments to insert into the formatted log message + * \param[in] args argument for the string format + */ +RCL_PUBLIC +void +rcl_logging_rosout_output_handler( + const rcutils_log_location_t * location, + int severity, + const char * name, + rcutils_time_point_value_t timestamp, + const char * format, + va_list * args); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__LOGGING_ROSOUT_H_ diff --git a/include/rcl/macros.h b/include/rcl/macros.h new file mode 100644 index 0000000..4df9ff4 --- /dev/null +++ b/include/rcl/macros.h @@ -0,0 +1,34 @@ +// Copyright 2015 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 RCL__MACROS_H_ +#define RCL__MACROS_H_ + +#include "rcutils/macros.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// Ignored return values of functions with this macro will emit a warning. +#define RCL_WARN_UNUSED RCUTILS_WARN_UNUSED + +#define RCL_UNUSED(x) RCUTILS_UNUSED(x) + +#ifdef __cplusplus +} +#endif + +#endif // RCL__MACROS_H_ diff --git a/include/rcl/network_flow_endpoints.h b/include/rcl/network_flow_endpoints.h new file mode 100644 index 0000000..4b09881 --- /dev/null +++ b/include/rcl/network_flow_endpoints.h @@ -0,0 +1,136 @@ +// Copyright 2020 Ericsson AB +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 RCL__NETWORK_FLOW_ENDPOINTS_H_ +#define RCL__NETWORK_FLOW_ENDPOINTS_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include + +#include "rcl/allocator.h" +#include "rcl/arguments.h" +#include "rcl/context.h" +#include "rcl/macros.h" +#include "rcl/publisher.h" +#include "rcl/subscription.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +typedef rmw_network_flow_endpoint_t rcl_network_flow_endpoint_t; +typedef rmw_network_flow_endpoint_array_t rcl_network_flow_endpoint_array_t; +typedef rmw_transport_protocol_t rcl_transport_protocol_t; +typedef rmw_internet_protocol_t rcl_internet_protocol_t; + +#define rcl_get_zero_initialized_network_flow_endpoint_array \ + rmw_get_zero_initialized_network_flow_endpoint_array +#define rcl_network_flow_endpoint_array_fini rmw_network_flow_endpoint_array_fini + +#define rcl_network_flow_endpoint_get_transport_protocol_string \ + rmw_network_flow_endpoint_get_transport_protocol_string +#define rcl_network_flow_endpoint_get_internet_protocol_string \ + rmw_network_flow_endpoint_get_internet_protocol_string + +/// Get network flow endpoints of a publisher +/** + * Query the underlying middleware for a given publisher's network flow endpoints + * + * The `publisher` argument must point to a valid publisher. + * + * The `allocator` argument must be a valid allocator. + * + * The `network_flow_endpoint_array` argument must be allocated and zero-initialized. + * The function returns network flow endpoints in the `network_flow_endpoint_array` argument, + * using the allocator to allocate memory for the `network_flow_endpoint_array` + * argument's internal data structures whenever required. The caller is + * reponsible for memory deallocation by passing the `network_flow_endpoint_array` + * argument to `rcl_network_flow_endpoint_array_fini` function. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] publisher the publisher instance to inspect + * \param[in] allocator allocator to be used when allocating space for network_flow_endpoint_array_t + * \param[out] network_flow_endpoint_array the network flow endpoints + * \return `RCL_RET_OK` if successful, or + * \return `RCL_RET_INVALID_ARGUMENT` if any argument is null, or + * \return `RCL_RET_BAD_ALLOC` if memory allocation fails, or + * \return `RCL_RET_UNSUPPORTED` if not supported, or + * \return `RCL_RET_ERROR` if an unexpected error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_publisher_get_network_flow_endpoints( + const rcl_publisher_t * publisher, + rcutils_allocator_t * allocator, + rcl_network_flow_endpoint_array_t * network_flow_endpoint_array); + +/// Get network flow endpoints of a subscription +/** + * Query the underlying middleware for a given subscription's network flow endpoints + * + * The `subscription` argument must point to a valid subscription. + * + * The `allocator` argument must be a valid allocator. + * + * The `network_flow_endpoint_array` argument must be allocated and zero-initialized. + * The function returns network flow endpoints in the `network_flow_endpoint_array` argument, + * using the allocator to allocate memory for the `network_flow_endpoint_array` + * argument's internal data structures whenever required. The caller is + * reponsible for memory deallocation by passing the `network_flow_endpoint_array` + * argument to `rcl_network_flow_endpoint_array_fini` function. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Maybe [1] + * [1] implementation may need to protect the data structure with a lock + * + * \param[in] subscription the subscription instance to inspect + * \param[in] allocator allocator to be used when allocating space for network_flow_endpoint_array_t + * \param[out] network_flow_endpoint_array the network flow endpoints + * \return `RCL_RET_OK` if successful, or + * \return `RCL_RET_INVALID_ARGUMENT` if any argument is null, or + * \return `RCL_RET_BAD_ALLOC` if memory allocation fails, or + * \return `RCL_RET_UNSUPPORTED` if not supported, or + * \return `RCL_RET_ERROR` if an unexpected error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_get_network_flow_endpoints( + const rcl_subscription_t * subscription, + rcutils_allocator_t * allocator, + rcl_network_flow_endpoint_array_t * network_flow_endpoint_array); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__NETWORK_FLOW_ENDPOINTS_H_ diff --git a/include/rcl/node.h b/include/rcl/node.h new file mode 100644 index 0000000..b9b8643 --- /dev/null +++ b/include/rcl/node.h @@ -0,0 +1,556 @@ +// Copyright 2015 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. + +/// @file + +#ifndef RCL__NODE_H_ +#define RCL__NODE_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +#include "rcl/allocator.h" +#include "rcl/arguments.h" +#include "rcl/context.h" +#include "rcl/guard_condition.h" +#include "rcl/macros.h" +#include "rcl/node_options.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +extern const char * const RCL_DISABLE_LOANED_MESSAGES_ENV_VAR; + +typedef struct rcl_node_impl_s rcl_node_impl_t; + +/// Structure which encapsulates a ROS Node. +typedef struct rcl_node_s +{ + /// Context associated with this node. + rcl_context_t * context; + + /// Private implementation pointer. + rcl_node_impl_t * impl; +} rcl_node_t; + +/// Return a rcl_node_t struct with members initialized to `NULL`. +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_node_t +rcl_get_zero_initialized_node(void); + +/// Initialize a ROS node. +/** + * Calling this on a rcl_node_t makes it a valid node handle until rcl_shutdown + * is called or until rcl_node_fini is called on it. + * + * After calling, the ROS node object can be used to create other middleware + * primitives like publishers, services, parameters, etc. + * + * The name of the node must not be NULL and adhere to naming restrictions, + * see the rmw_validate_node_name() function for rules. + * + * \todo TODO(wjwwood): node name uniqueness is not yet enforced + * + * The name of the node cannot coincide with another node of the same name. + * If a node of the same name is already in the domain, it will be shutdown. + * + * The namespace of the node should not be NULL and should also pass the + * rmw_validate_namespace() function's rules. + * + * Additionally this function allows namespaces which lack a leading forward + * slash. + * Because there is no notion of a relative namespace, there is no difference + * between a namespace which lacks a forward and the same namespace with a + * leading forward slash. + * Therefore, a namespace like ``"foo/bar"`` is automatically changed to + * ``"/foo/bar"`` by this function. + * Similarly, the namespace ``""`` will implicitly become ``"/"`` which is a + * valid namespace. + * + * \todo TODO(wjwwood): + * Parameter infrastructure is currently initialized in the language specific + * client library, e.g. rclcpp for C++, but will be initialized here in the + * future. When that happens there will be an option to avoid parameter + * infrastructure with an option in the rcl_node_options_t struct. + * + * A node contains infrastructure for ROS parameters, which include advertising + * publishers and service servers. + * This function will create those external parameter interfaces even if + * parameters are not used later. + * + * The rcl_node_t given must be allocated and zero initialized. + * Passing an rcl_node_t which has already had this function called on it, more + * recently than rcl_node_fini, will fail. + * An allocated rcl_node_t with uninitialized memory is undefined behavior. + * + * Expected usage: + * + * ```c + * rcl_context_t context = rcl_get_zero_initialized_context(); + * // ... initialize the context with rcl_init() + * rcl_node_t node = rcl_get_zero_initialized_node(); + * rcl_node_options_t node_ops = rcl_node_get_default_options(); + * // ... node options customization + * rcl_ret_t ret = rcl_node_init(&node, "node_name", "/node_ns", &context, &node_ops); + * // ... error handling and then use the node, but eventually deinitialize it: + * ret = rcl_node_fini(&node); + * // ... error handling for rcl_node_fini() + * ``` + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_uint_least64_t` + * + * \pre the node handle must be allocated, zero initialized, and invalid + * \pre the context handle must be allocated, initialized, and valid + * \post the node handle is valid and can be used in other `rcl_*` functions + * + * \param[inout] node a preallocated rcl_node_t + * \param[in] name the name of the node, must be a valid c-string + * \param[in] namespace_ the namespace of the node, must be a valid c-string + * \param[in] context the context instance with which the node should be + * associated + * \param[in] options the node options. + * The options are deep copied into the node. + * The caller is always responsible for freeing memory used options they + * pass in. + * \return #RCL_RET_OK if the node was initialized successfully, or + * \return #RCL_RET_ALREADY_INIT if the node has already be initialized, or + * \return #RCL_RET_NOT_INIT if the given context is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_NODE_INVALID_NAME if the name is invalid, or + * \return #RCL_RET_NODE_INVALID_NAMESPACE if the namespace_ is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_node_init( + rcl_node_t * node, + const char * name, + const char * namespace_, + rcl_context_t * context, + const rcl_node_options_t * options); + +/// Finalize a rcl_node_t. +/** + * Destroys any automatically created infrastructure and deallocates memory. + * After calling, the rcl_node_t can be safely deallocated. + * + * All middleware primitives created by the user, e.g. publishers, services, etc, + * which were created from this node must be finalized using their respective + * `rcl_*_fini()` functions before this is called. + * \sa rcl_publisher_fini() + * \sa rcl_subscription_fini() + * \sa rcl_client_fini() + * \sa rcl_service_fini() + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_uint_least64_t` + * + * \param[in] node rcl_node_t to be finalized + * \return #RCL_RET_OK if node was finalized successfully, or + * \return #RCL_RET_NODE_INVALID if the node pointer is null, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_node_fini(rcl_node_t * node); + +/// Return `true` if the node is valid, else `false`. +/** + * Also return `false` if the node pointer is `NULL` or the allocator is invalid. + * + * A node is invalid if: + * - the implementation is `NULL` (rcl_node_init not called or failed) + * - rcl_shutdown has been called since the node has been initialized + * - the node has been finalized with rcl_node_fini + * + * There is a possible validity race condition. + * + * Consider: + * + * ```c + * assert(rcl_node_is_valid(node)); // <-- thread 1 + * rcl_shutdown(); // <-- thread 2 + * // use node as if valid // <-- thread 1 + * ``` + * + * In the third line the node is now invalid, even though on the previous line + * of thread 1 it was checked to be valid. + * This is why this function is considered not thread-safe. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_uint_least64_t` + * + * \param[in] node rcl_node_t to be validated + * \return `true` if the node and allocator are valid, otherwise `false`. + */ +RCL_PUBLIC +bool +rcl_node_is_valid(const rcl_node_t * node); + +/// Return true if node is valid, except for the context being valid. +/** + * This is used in clean up functions that need to access the node, but do not + * need use any functions with the context. + * + * It is identical to rcl_node_is_valid except it ignores the state of the + * context associated with the node. + * \sa rcl_node_is_valid() + */ +RCL_PUBLIC +bool +rcl_node_is_valid_except_context(const rcl_node_t * node); + +/// Return the name of the node. +/** + * This function returns the node's internal name string. + * This function can fail, and therefore return `NULL`, if: + * - node is `NULL` + * - node has not been initialized (the implementation is invalid) + * + * The returned string is only valid as long as the given rcl_node_t is valid. + * The value of the string may change if the value in the rcl_node_t changes, + * and therefore copying the string is recommended if this is a concern. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node pointer to the node + * \return name string if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_node_get_name(const rcl_node_t * node); + +/// Return the namespace of the node. +/** + * This function returns the node's internal namespace string. + * This function can fail, and therefore return `NULL`, if: + * - node is `NULL` + * - node has not been initialized (the implementation is invalid) + * + * The returned string is only valid as long as the given rcl_node_t is valid. + * The value of the string may change if the value in the rcl_node_t changes, + * and therefore copying the string is recommended if this is a concern. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node pointer to the node + * \return name string if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_node_get_namespace(const rcl_node_t * node); + +/// Return the fully qualified name of the node. +/** + * This function returns the node's internal namespace and name combined string. + * This function can fail, and therefore return `NULL`, if: + * - node is `NULL` + * - node has not been initialized (the implementation is invalid) + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node pointer to the node + * \return fully qualified name string if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_node_get_fully_qualified_name(const rcl_node_t * node); + +/// Return the rcl node options. +/** + * This function returns the node's internal options struct. + * This function can fail, and therefore return `NULL`, if: + * - node is `NULL` + * - node has not been initialized (the implementation is invalid) + * + * The returned struct is only valid as long as the given rcl_node_t is valid. + * The values in the struct may change if the options of the rcl_node_t changes, + * and therefore copying the struct is recommended if this is a concern. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node pointer to the node + * \return options struct if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rcl_node_options_t * +rcl_node_get_options(const rcl_node_t * node); + +/// Return the ROS domain ID that the node is using. +/** + * This function returns the ROS domain ID that the node is in. + * + * This function should be used to determine what `domain_id` was used rather + * than checking the domain_id field in the node options, because if + * #RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID is used when creating the node then + * it is not changed after creation, but this function will return the actual + * `domain_id` used. + * + * The `domain_id` field must point to an allocated `size_t` object to which + * the ROS domain ID will be written. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node the handle to the node being queried + * \param[out] domain_id storage for the domain id + * \return #RCL_RET_OK if node the domain ID was retrieved successfully, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id); + +/// Return the rmw node handle. +/** + * The handle returned is a pointer to the internally held rmw handle. + * This function can fail, and therefore return `NULL`, if: + * - node is `NULL` + * - node has not been initialized (the implementation is invalid) + * + * The returned handle is made invalid if the node is finalized or if + * rcl_shutdown() is called. + * The returned handle is not guaranteed to be valid for the life time of the + * node as it may be finalized and recreated itself. + * Therefore it is recommended to get the handle from the node using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node pointer to the rcl node + * \return rmw node handle if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_node_t * +rcl_node_get_rmw_handle(const rcl_node_t * node); + +/// Return the associated rcl instance id. +/** + * This id is stored when rcl_node_init is called and can be compared with the + * value returned by rcl_get_instance_id() to check if this node was created in + * the current rcl context (since the latest call to rcl_init(). + * + * This function can fail, and therefore return `0`, if: + * - node is `NULL` + * - node has not been initialized (the implementation is invalid) + * + * This function will succeed even if rcl_shutdown() has been called + * since the node was created. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node pointer to the rcl node + * \return rcl instance id captured during node init or `0` on error + */ +RCL_PUBLIC +RCL_WARN_UNUSED +uint64_t +rcl_node_get_rcl_instance_id(const rcl_node_t * node); + +/// Return a guard condition which is triggered when the ROS graph changes. +/** + * The handle returned is a pointer to an internally held rcl guard condition. + * This function can fail, and therefore return `NULL`, if: + * - node is `NULL` + * - node is invalid + * + * The returned handle is made invalid if the node is finialized or if + * rcl_shutdown() is called. + * + * The guard condition will be triggered anytime a change to the ROS graph occurs. + * A ROS graph change includes things like (but not limited to) a new publisher + * advertises, a new subscription is created, a new service becomes available, + * a subscription is canceled, etc. + * + * \todo TODO(wjwwood): link to exhaustive list of graph events + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node pointer to the rcl node + * \return rcl guard condition handle if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rcl_guard_condition_t * +rcl_node_get_graph_guard_condition(const rcl_node_t * node); + +/// Return the logger name of the node. +/** + * This function returns the node's internal logger name string. + * This function can fail, and therefore return `NULL`, if: + * - node is `NULL` + * - node has not been initialized (the implementation is invalid) + * + * The returned string is only valid as long as the given rcl_node_t is valid. + * The value of the string may change if the value in the rcl_node_t changes, + * and therefore copying the string is recommended if this is a concern. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node pointer to the node + * \return logger_name string if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_node_get_logger_name(const rcl_node_t * node); + +/// Expand a given name into a fully-qualified topic name and apply remapping rules. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] node Node object. Its name, namespace, local/global command line arguments are used. + * \param[in] input_name Topic name to be expanded and remapped. + * \param[in] allocator The allocator to be used when creating the output topic. + * \param[in] is_service For services use `true`, for topics use `false`. + * \param[in] only_expand When `true`, remapping rules are ignored. + * \param[out] output_name Output char * pointer. + * \return #RCL_RET_OK if the topic name was expanded successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any of input_name, node_name, node_namespace + * or output_name are NULL, or + * \return #RCL_RET_INVALID_ARGUMENT if both local_args and global_args are NULL, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_TOPIC_NAME_INVALID if the given topic name is invalid + * (see rcl_validate_topic_name()), or + * \return #RCL_RET_NODE_INVALID_NAME if the given node name is invalid + * (see rmw_validate_node_name()), or + * \return #RCL_RET_NODE_INVALID_NAMESPACE if the given node namespace is invalid + * (see rmw_validate_namespace()), or + * \return #RCL_RET_UNKNOWN_SUBSTITUTION for unknown substitutions in name, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_node_resolve_name( + const rcl_node_t * node, + const char * input_name, + rcl_allocator_t allocator, + bool is_service, + bool only_expand, + char ** output_name); + +/// Check if loaned message is disabled, according to the environment variable. +/** + * If the `ROS_DISABLE_LOANED_MESSAGES` environment variable is set to "1", + * `disable_loaned_message` will be set to true. + * + * \param[out] disable_loaned_message Must not be NULL. + * \return #RCL_RET_INVALID_ARGUMENT if an argument is not valid, or + * \return #RCL_RET_ERROR if an unexpected error happened, or + * \return #RCL_RET_OK. + */ +RCL_PUBLIC +rcl_ret_t +rcl_get_disable_loaned_message(bool * disable_loaned_message); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__NODE_H_ diff --git a/include/rcl/node_options.h b/include/rcl/node_options.h new file mode 100644 index 0000000..551d794 --- /dev/null +++ b/include/rcl/node_options.h @@ -0,0 +1,127 @@ +// 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. + +/// @file + +#ifndef RCL__NODE_OPTIONS_H_ +#define RCL__NODE_OPTIONS_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/allocator.h" +#include "rcl/arguments.h" + +#include "rcl/domain_id.h" + +/// Constant which indicates that the default domain id should be used. +#define RCL_NODE_OPTIONS_DEFAULT_DOMAIN_ID RCL_DEFAULT_DOMAIN_ID + +/// Structure which encapsulates the options for creating a rcl_node_t. +typedef struct rcl_node_options_s +{ + // bool anonymous_name; + + // rmw_qos_profile_t parameter_qos; + + /// If true, no parameter infrastructure will be setup. + // bool no_parameters; + + /// Custom allocator used for internal allocations. + rcl_allocator_t allocator; + + /// If false then only use arguments in this struct, otherwise use global arguments also. + bool use_global_arguments; + + /// Command line arguments that apply only to this node. + rcl_arguments_t arguments; + + /// Flag to enable rosout for this node + bool enable_rosout; + + /// Middleware quality of service settings for /rosout. + rmw_qos_profile_t rosout_qos; +} rcl_node_options_t; + +/// Return the default node options in a rcl_node_options_t. +/** + * The default values are: + * + * - allocator = rcl_get_default_allocator() + * - use_global_arguments = true + * - enable_rosout = true + * - arguments = rcl_get_zero_initialized_arguments() + * - rosout_qos = rcl_qos_profile_rosout_default + * + * \return A structure with the default node options. + */ +RCL_PUBLIC +rcl_node_options_t +rcl_node_get_default_options(void); + +/// Copy one options structure into another. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] options The structure to be copied. + * Its allocator is used to copy memory into the new structure. + * \param[out] options_out An options structure containing default values. + * \return #RCL_RET_OK if the structure was copied successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_node_options_copy( + const rcl_node_options_t * options, + rcl_node_options_t * options_out); + +/// Finalize the given node_options. +/** + * The given node_options must be non-`NULL` and valid, i.e. had + * rcl_node_get_default_options() called on it but not this function yet. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Yes + * Lock-Free | Yes + * + * \param[inout] options object to be finalized + * \return #RCL_RET_OK if setup is successful, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_node_options_fini(rcl_node_options_t * options); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__NODE_OPTIONS_H_ diff --git a/include/rcl/publisher.h b/include/rcl/publisher.h new file mode 100644 index 0000000..c39f5cf --- /dev/null +++ b/include/rcl/publisher.h @@ -0,0 +1,694 @@ +// Copyright 2015 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. + +/// @file + +#ifndef RCL__PUBLISHER_H_ +#define RCL__PUBLISHER_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rosidl_runtime_c/message_type_support_struct.h" + +#include "rcl/macros.h" +#include "rcl/node.h" +#include "rcl/visibility_control.h" +#include "rcl/time.h" + +/// Internal rcl publisher implementation struct. +typedef struct rcl_publisher_impl_s rcl_publisher_impl_t; + +/// Structure which encapsulates a ROS Publisher. +typedef struct rcl_publisher_s +{ + /// Pointer to the publisher implementation + rcl_publisher_impl_t * impl; +} rcl_publisher_t; + +/// Options available for a rcl publisher. +typedef struct rcl_publisher_options_s +{ + /// Middleware quality of service settings for the publisher. + rmw_qos_profile_t qos; + /// Custom allocator for the publisher, used for incidental allocations. + /** For default behavior (malloc/free), use: rcl_get_default_allocator() */ + rcl_allocator_t allocator; + /// rmw specific publisher options, e.g. the rmw implementation specific payload. + rmw_publisher_options_t rmw_publisher_options; +} rcl_publisher_options_t; + +/// Return a rcl_publisher_t struct with members set to `NULL`. +/** + * Should be called to get a null rcl_publisher_t before passing to + * rcl_publisher_init(). + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_publisher_t +rcl_get_zero_initialized_publisher(void); + +/// Initialize a rcl publisher. +/** + * After calling this function on a rcl_publisher_t, it can be used to publish + * messages of the given type to the given topic using rcl_publish(). + * + * The given rcl_node_t must be valid and the resulting rcl_publisher_t is only + * valid as long as the given rcl_node_t remains valid. + * + * The rosidl_message_type_support_t is obtained on a per .msg type basis. + * When the user defines a ROS message, code is generated which provides the + * required rosidl_message_type_support_t object. + * This object can be obtained using a language appropriate mechanism. + * \todo TODO(wjwwood) write these instructions once and link to it instead + * + * For C, a macro can be used (for example `std_msgs/String`): + * + * ```c + * #include + * #include + * const rosidl_message_type_support_t * string_ts = + * ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String); + * ``` + * + * For C++, a template function is used: + * + * ```cpp + * #include + * #include + * const rosidl_message_type_support_t * string_ts = + * rosidl_typesupport_cpp::get_message_type_support_handle(); + * ``` + * + * The rosidl_message_type_support_t object contains message type specific + * information used to publish messages. + * + * The topic name must be a c string which follows the topic and service name + * format rules for unexpanded names, also known as non-fully qualified names: + * + * \see rcl_expand_topic_name + * + * The options struct allows the user to set the quality of service settings as + * well as a custom allocator which is used when initializing/finalizing the + * publisher to allocate space for incidentals, e.g. the topic name string. + * + * Expected usage (for C messages): + * + * ```c + * #include + * #include + * #include + * + * rcl_node_t node = rcl_get_zero_initialized_node(); + * rcl_node_options_t node_ops = rcl_node_get_default_options(); + * rcl_ret_t ret = rcl_node_init(&node, "node_name", "/my_namespace", &node_ops); + * // ... error handling + * const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String); + * rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + * rcl_publisher_options_t publisher_ops = rcl_publisher_get_default_options(); + * ret = rcl_publisher_init(&publisher, &node, ts, "chatter", &publisher_ops); + * // ... error handling, and on shutdown do finalization: + * ret = rcl_publisher_fini(&publisher, &node); + * // ... error handling for rcl_publisher_fini() + * ret = rcl_node_fini(&node); + * // ... error handling for rcl_deinitialize_node() + * ``` + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] publisher preallocated publisher structure + * \param[in] node valid rcl node handle + * \param[in] type_support type support object for the topic's type + * \param[in] topic_name the name of the topic to publish on + * \param[in] options publisher options, including quality of service settings + * \return #RCL_RET_OK if the publisher was initialized successfully, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_ALREADY_INIT if the publisher is already initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory fails, or + * \return #RCL_RET_TOPIC_NAME_INVALID if the given topic name is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_publisher_init( + rcl_publisher_t * publisher, + const rcl_node_t * node, + const rosidl_message_type_support_t * type_support, + const char * topic_name, + const rcl_publisher_options_t * options); + +/// Finalize a rcl_publisher_t. +/** + * After calling, the node will no longer be advertising that it is publishing + * on this topic (assuming this is the only publisher on this topic). + * + * After calling, calls to rcl_publish will fail when using this publisher. + * However, the given node handle is still valid. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] publisher handle to the publisher to be finalized + * \param[in] node a valid (not finalized) handle to the node used to create the publisher + * \return #RCL_RET_OK if publisher was finalized successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node); + +/// Return the default publisher options in a rcl_publisher_options_t. +/** + * The defaults are: + * + * - qos = rmw_qos_profile_default + * - allocator = rcl_get_default_allocator() + * - rmw_publisher_options = rmw_get_default_publisher_options() + * + * \return A structure with the default publisher options. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_publisher_options_t +rcl_publisher_get_default_options(void); + +/// Borrow a loaned message. +/** + * The memory allocated for the ros message belongs to the middleware and must not be deallocated + * other than by a call to \sa rcl_return_loaned_message_from_publisher. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No [0] + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * [0] the underlying middleware might allocate new memory or returns an existing chunk form a pool. + * The function in rcl however does not allocate any additional memory. + * + * \param[in] publisher Publisher to which the allocated message is associated. + * \param[in] type_support Typesupport to which the internal ros message is allocated. + * \param[out] ros_message The pointer to be filled to a valid ros message by the middleware. + * \return #RCL_RET_OK if the ros message was correctly initialized, or + * \return #RCL_RET_PUBLISHER_INVALID if the passed publisher is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if an argument other than the ros message is null, or + * \return #RCL_RET_BAD_ALLOC if the ros message could not be correctly created, or + * \return #RCL_RET_UNSUPPORTED if the middleware does not support that feature, or + * \return #RCL_RET_ERROR if an unexpected error occured. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_borrow_loaned_message( + const rcl_publisher_t * publisher, + const rosidl_message_type_support_t * type_support, + void ** ros_message); + +/// Return a loaned message previously borrowed from a publisher. +/** + * The ownership of the passed in ros message will be transferred back to the middleware. + * The middleware might deallocate and destroy the message so that the pointer is no longer + * guaranteed to be valid after that call. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] publisher Publisher to which the loaned message is associated. + * \param[in] loaned_message Loaned message to be deallocated and destroyed. + * \return #RCL_RET_OK if successful, or + * \return #RCL_RET_INVALID_ARGUMENT if an argument is null, or + * \return #RCL_RET_UNSUPPORTED if the middleware does not support that feature, or + * \return #RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or + * \return #RCL_RET_ERROR if an unexpected error occurs and no message can be initialized. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_return_loaned_message_from_publisher( + const rcl_publisher_t * publisher, + void * loaned_message); + +/// Publish a ROS message on a topic using a publisher. +/** + * It is the job of the caller to ensure that the type of the ros_message + * parameter and the type associate with the publisher (via the type support) + * match. + * Passing a different type to publish produces undefined behavior and cannot + * be checked by this function and therefore no deliberate error will occur. + * + * \todo TODO(wjwwood): + * The blocking behavior of publish is a still a point of dispute. + * This section should be updated once the behavior is clearly defined. + * See: https://github.com/ros2/ros2/issues/255 + * + * Calling rcl_publish() is a potentially blocking call. + * When called rcl_publish() will immediately do any publishing related work, + * including, but not limited to, converting the message into a different type, + * serializing the message, collecting publish statistics, etc. + * The last thing it will do is call the underlying middleware's publish + * function which may or may not block based on the quality of service settings + * given via the publisher options in rcl_publisher_init(). + * For example, if the reliability is set to reliable, then a publish may block + * until space in the publish queue is available, but if the reliability is set + * to best effort then it should not block. + * + * The ROS message given by the `ros_message` void pointer is always owned by + * the calling code, but should remain constant during publish. + * + * This function is thread safe so long as access to both the publisher and the + * `ros_message` is synchronized. + * That means that calling rcl_publish() from multiple threads is allowed, but + * calling rcl_publish() at the same time as non-thread safe publisher + * functions is not, e.g. calling rcl_publish() and rcl_publisher_fini() + * concurrently is not allowed. + * Before calling rcl_publish() the message can change and after calling + * rcl_publish() the message can change, but it cannot be changed during the + * publish call. + * The same `ros_message`, however, can be passed to multiple calls of + * rcl_publish() simultaneously, even if the publishers differ. + * The `ros_message` is unmodified by rcl_publish(). + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes [1] + * Uses Atomics | No + * Lock-Free | Yes + * [1] for unique pairs of publishers and messages, see above for more + * + * \param[in] publisher handle to the publisher which will do the publishing + * \param[in] ros_message type-erased pointer to the ROS message + * \param[in] allocation structure pointer, used for memory preallocation (may be NULL) + * \return #RCL_RET_OK if the message was published successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_publish( + const rcl_publisher_t * publisher, + const void * ros_message, + rmw_publisher_allocation_t * allocation); + +/// Publish a serialized message on a topic using a publisher. +/** + * It is the job of the caller to ensure that the type of the serialized message + * parameter and the type associate with the publisher (via the type support) + * match. + * Even though this call to publish takes an already serialized serialized message, + * the publisher has to register its type as a ROS known message type. + * Passing a serialized message from a different type leads to undefined behavior on the subscriber side. + * The publish call might be able to send any abitrary serialized message, it is however + * not garantueed that the subscriber side successfully deserializes this byte stream. + * + * Apart from this, the `publish_serialized` function has the same behavior as rcl_publish() + * expect that no serialization step is done. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes [1] + * Uses Atomics | No + * Lock-Free | Yes + * [1] for unique pairs of publishers and messages, see above for more + * + * \param[in] publisher handle to the publisher which will do the publishing + * \param[in] serialized_message pointer to the already serialized message in raw form + * \param[in] allocation structure pointer, used for memory preallocation (may be NULL) + * \return #RCL_RET_OK if the message was published successfully, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_publish_serialized_message( + const rcl_publisher_t * publisher, + const rcl_serialized_message_t * serialized_message, + rmw_publisher_allocation_t * allocation); + +/// Publish a loaned message on a topic using a publisher. +/** + * A previously borrowed loaned message can be sent via this call to rcl_publish_loaned_message(). + * By calling this function, the ownership of the loaned message is getting transferred back + * to the middleware. + * The pointer to the `ros_message` is not guaranteed to be valid after as the middleware + * migth deallocate the memory for this message internally. + * It is thus recommended to call this function only in combination with + * \sa rcl_borrow_loaned_message(). + * + * Apart from this, the `publish_loaned_message` function has the same behavior as rcl_publish() + * except that no serialization step is done. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No [0] + * Thread-Safe | Yes [1] + * Uses Atomics | No + * Lock-Free | Yes + * [0] the middleware might deallocate the loaned message. + * The RCL function however does not allocate any memory. + * [1] for unique pairs of publishers and messages, see above for more + * + * \param[in] publisher handle to the publisher which will do the publishing + * \param[in] ros_message pointer to the previously borrow loaned message + * \param[in] allocation structure pointer, used for memory preallocation (may be NULL) + * \return #RCL_RET_OK if the message was published successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or + * \return #RCL_RET_UNSUPPORTED if the middleware does not support that feature, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_publish_loaned_message( + const rcl_publisher_t * publisher, + void * ros_message, + rmw_publisher_allocation_t * allocation); + +/// Manually assert that this Publisher is alive (for RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC) +/** + * If the rmw Liveliness policy is set to RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, the creator of + * this publisher may manually call `assert_liveliness` at some point in time to signal to the rest + * of the system that this Node is still alive. + * This function must be called at least as often as the qos_profile's liveliness_lease_duration + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] publisher handle to the publisher that needs liveliness to be asserted + * \return #RCL_RET_OK if the liveliness assertion was completed successfully, or + * \return #RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_publisher_assert_liveliness(const rcl_publisher_t * publisher); + +/// Wait until all published message data is acknowledged or until the specified timeout elapses. +/** + * This function waits until all published message data were acknowledged by peer node or timeout. + * + * The timeout unit is nanoseconds. + * If the timeout is negative then this function will block indefinitely until all published message + * data were acknowledged. + * If the timeout is 0 then this function will be non-blocking; checking all published message data + * were acknowledged (If acknowledged, return RCL_RET_OK. Otherwise, return RCL_RET_TIMEOUT), but + * not waiting. + * If the timeout is greater than 0 then this function will return after that period of time has + * elapsed (return RCL_RET_TIMEOUT) or all published message data were acknowledged (return + * RCL_RET_OK). + * + * This function only waits for acknowledgments if the publisher's QOS profile is RELIABLE. + * Otherwise this function will immediately return RCL_RET_OK. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] publisher handle to the publisher that needs to wait for all acked. + * \param[in] timeout the duration to wait for all published message data were acknowledged, in + * nanoseconds. + * \return #RCL_RET_OK if successful, or + * \return #RCL_RET_TIMEOUT if timed out, or + * \return #RCL_RET_PUBLISHER_INVALID if publisher is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs, or + * \return #RCL_RET_UNSUPPORTED if the middleware does not support that feature. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_publisher_wait_for_all_acked( + const rcl_publisher_t * publisher, + rcl_duration_value_t timeout); + +/// Get the topic name for the publisher. +/** + * This function returns the publisher's internal topic name string. + * This function can fail, and therefore return `NULL`, if the: + * - publisher is `NULL` + * - publisher is invalid (never called init, called fini, or invalid node) + * + * The returned string is only valid as long as the rcl_publisher_t is valid. + * The value of the string may change if the topic name changes, and therefore + * copying the string is recommended if this is a concern. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] publisher pointer to the publisher + * \return name string if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_publisher_get_topic_name(const rcl_publisher_t * publisher); + +/// Return the rcl publisher options. +/** + * This function returns the publisher's internal options struct. + * This function can fail, and therefore return `NULL`, if the: + * - publisher is `NULL` + * - publisher is invalid (never called init, called fini, or invalid node) + * + * The returned struct is only valid as long as the rcl_publisher_t is valid. + * The values in the struct may change if the options of the publisher change, + * and therefore copying the struct is recommended if this is a concern. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] publisher pointer to the publisher + * \return options struct if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rcl_publisher_options_t * +rcl_publisher_get_options(const rcl_publisher_t * publisher); + +/// Return the rmw publisher handle. +/** + * The handle returned is a pointer to the internally held rmw handle. + * This function can fail, and therefore return `NULL`, if the: + * - publisher is `NULL` + * - publisher is invalid (never called init, called fini, or invalid node) + * + * The returned handle is made invalid if the publisher is finalized or if + * rcl_shutdown() is called. + * The returned handle is not guaranteed to be valid for the life time of the + * publisher as it may be finalized and recreated itself. + * Therefore it is recommended to get the handle from the publisher using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] publisher pointer to the rcl publisher + * \return rmw publisher handle if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_publisher_t * +rcl_publisher_get_rmw_handle(const rcl_publisher_t * publisher); + +/// Return the context associated with this publisher. +/** + * This function can fail, and therefore return `NULL`, if the: + * - publisher is `NULL` + * - publisher is invalid (never called init, called fini, etc.) + * + * The returned context is made invalid if the publisher is finalized or if + * rcl_shutdown() is called. + * Therefore it is recommended to get the handle from the publisher using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] publisher pointer to the rcl publisher + * \return context if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_context_t * +rcl_publisher_get_context(const rcl_publisher_t * publisher); + +/// Return true if the publisher is valid, otherwise false. +/** + * The bool returned is `false` if `publisher` is invalid. + * The bool returned is `true` otherwise. + * In the case where `false` is to be returned, an error message is set. + * This function cannot fail. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] publisher pointer to the rcl publisher + * \return `true` if `publisher` is valid, otherwise `false` + */ +RCL_PUBLIC +bool +rcl_publisher_is_valid(const rcl_publisher_t * publisher); + +/// Return true if the publisher is valid except the context, otherwise false. +/** + * This is used in clean up functions that need to access the publisher, but do + * not need use any functions with the context. + * + * It is identical to rcl_publisher_is_valid except it ignores the state of the + * context associated with the publisher. + * \sa rcl_publisher_is_valid() + */ +RCL_PUBLIC +bool +rcl_publisher_is_valid_except_context(const rcl_publisher_t * publisher); + +/// Get the number of subscriptions matched to a publisher. +/** + * Used to get the internal count of subscriptions matched to a publisher. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] only if the underlying rmw doesn't make use of this feature + * + * \param[in] publisher pointer to the rcl publisher + * \param[out] subscription_count number of matched subscriptions + * \return #RCL_RET_OK if the count was retrieved, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_PUBLISHER_INVALID if the publisher is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_publisher_get_subscription_count( + const rcl_publisher_t * publisher, + size_t * subscription_count); + +/// Get the actual qos settings of the publisher. +/** + * Used to get the actual qos settings of the publisher. + * The actual configuration applied when using RMW_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the publisher, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_*_UNKNOWN. + * The returned struct is only valid as long as the rcl_publisher_t is valid. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] publisher pointer to the rcl publisher + * \return qos struct if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rmw_qos_profile_t * +rcl_publisher_get_actual_qos(const rcl_publisher_t * publisher); + + +/// Check if publisher instance can loan messages. +/** + * Depending on the middleware and the message type, this will return true if the middleware + * can allocate a ROS message instance. + */ +RCL_PUBLIC +bool +rcl_publisher_can_loan_messages(const rcl_publisher_t * publisher); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__PUBLISHER_H_ diff --git a/include/rcl/rcl.h b/include/rcl/rcl.h new file mode 100644 index 0000000..b63e6a9 --- /dev/null +++ b/include/rcl/rcl.h @@ -0,0 +1,86 @@ +// Copyright 2014 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. + +/** \mainpage rcl: Common functionality for other ROS Client Libraries + * + * `rcl` consists of functions and structs (pure C) organized into ROS concepts: + * + * - Nodes + * - rcl/node.h + * - Publisher + * - rcl/publisher.h + * - Subscription + * - rcl/subscription.h + * - Service Client + * - rcl/client.h + * - Service Server + * - rcl/service.h + * - Timer + * - rcl/timer.h + * + * There are some functions for working with "Topics" and "Services": + * + * - A function to validate a topic or service name (not necessarily fully qualified): + * - rcl_validate_topic_name() + * - rcl/validate_topic_name.h + * - A function to expand a topic or service name to a fully qualified name: + * - rcl_expand_topic_name() + * - rcl/expand_topic_name.h + * + * It also has some machinery that is necessary to wait on and act on these concepts: + * + * - Initialization and shutdown management + * - rcl/init.h + * - Wait sets for waiting on messages/service requests and responses/timers to be ready + * - rcl/wait.h + * - Guard conditions for waking up wait sets asynchronously + * - rcl/guard_condition.h + * - Functions for introspecting and getting notified of changes of the ROS graph + * - rcl/graph.h + * + * Further still there are some useful abstractions and utilities: + * + * - Allocator concept, which can be used to control allocation in `rcl_*` functions + * - rcl/allocator.h + * - Concept of ROS Time and access to steady and system wall time + * - rcl/time.h + * - Error handling functionality (C style) + * - rcl/error_handling.h + * - Macros + * - rcl/macros.h + * - Return code types + * - rcl/types.h + * - Macros for controlling symbol visibility on the library + * - rcl/visibility_control.h + */ + +#ifndef RCL__RCL_H_ +#define RCL__RCL_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/init.h" +#include "rcl/node.h" +#include "rcl/publisher.h" +#include "rcl/subscription.h" +#include "rcl/wait.h" + +#ifdef __cplusplus +} +#endif + +#endif // RCL__RCL_H_ diff --git a/include/rcl/remap.h b/include/rcl/remap.h new file mode 100644 index 0000000..5acf5e4 --- /dev/null +++ b/include/rcl/remap.h @@ -0,0 +1,302 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file + +#ifndef RCL__REMAP_H_ +#define RCL__REMAP_H_ + +#include "rcl/allocator.h" +#include "rcl/arguments.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +typedef struct rcl_remap_impl_s rcl_remap_impl_t; + +/// Hold remapping rules. +typedef struct rcl_remap_s +{ + /// Private implementation pointer. + rcl_remap_impl_t * impl; +} rcl_remap_t; + +/// Return a rcl_remap_t struct with members initialized to `NULL`. +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_remap_t +rcl_get_zero_initialized_remap(void); + +// TODO(sloretz) add documentation about rostopic:// when it is supported +/// Remap a topic name based on given rules. +/** + * The supplied topic name must have already been expanded to a fully qualified name. + * \sa rcl_expand_topic_name() + * + * If `local_arguments` is not NULL and not zero intialized then its remap rules are checked first. + * If no rules matched and `global_arguments` is not NULL and not zero intitialized then its rules + * are checked next. + * If both `local_arguments` and global_arguments are NULL or zero intialized then the function will + * return RCL_RET_INVALID_ARGUMENT. + * + * `global_arguments` is usually the arguments passed to rcl_init(). + * \sa rcl_init() + * \sa rcl_get_global_arguments() + * + * Remap rules are checked in the order they were given. + * For rules passed to rcl_init() this usually is the order they were passed on the command line. + * \sa rcl_parse_arguments() + * + * Only the first remap rule that matches is used to remap a name. + * For example, if the command line arguments are `foo:=bar bar:=baz` the topic `foo` is remapped to + * `bar` and not `baz`. + * + * `node_name` and `node_namespace` are used to expand the match and replacement into fully + * qualified names. + * Given node_name `trudy`, namespace `/ns`, and rule `foo:=~/bar` the names in the rule are + * expanded to `/ns/foo:=/ns/trudy/bar`. + * The rule will only apply if the given topic name is `/ns/foo`. + * + * `node_name` is also used to match against node specific rules. + * Given rules `alice:foo:=bar foo:=baz`, node name `alice`, and topic `foo` the remapped topic + * name will be `bar`. + * If given the node name `bob` and topic `foo` the remaped topic name would be `baz` instead. + * Note that processing always stops at the first matching rule even if there is a more specific one + * later on. + * Given `foo:=bar alice:foo:=baz` and topic name `foo` the remapped topic name will always be + * `bar` regardless of the node name given. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] local_arguments Command line arguments to be used before global arguments, or + * if NULL or zero-initialized then only global arguments are used. + * \param[in] global_arguments Command line arguments to use if no local rules matched, or + * `NULL` or zero-initialized to ignore global arguments. + * \param[in] topic_name A fully qualified and expanded topic name to be remapped. + * \param[in] node_name The name of the node to which name belongs. + * \param[in] node_namespace The namespace of a node to which name belongs. + * \param[in] allocator A valid allocator to use. + * \param[out] output_name Either an allocated string with the remapped name, or + * `NULL` if no remap rules matched the name. + * \return #RCL_RET_OK if the topic name was remapped or no rules matched, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_TOPIC_NAME_INVALID if the given topic name is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_remap_topic_name( + const rcl_arguments_t * local_arguments, + const rcl_arguments_t * global_arguments, + const char * topic_name, + const char * node_name, + const char * node_namespace, + rcl_allocator_t allocator, + char ** output_name); + +// TODO(sloretz) add documentation about rosservice:// when it is supported +/// Remap a service name based on given rules. +/** + * The supplied service name must have already been expanded to a fully qualified name. + * + * The behavior of this function is identical to rcl_expand_topic_name() except that it applies + * to service names instead of topic names. + * \sa rcl_expand_topic_name() + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] local_arguments Command line arguments to be used before global arguments, or + * if NULL or zero-initialized then only global arguments are used. + * \param[in] global_arguments Command line arguments to use if no local rules matched, or + * `NULL` or zero-initialized to ignore global arguments. + * \param[in] service_name A fully qualified and expanded service name to be remapped. + * \param[in] node_name The name of the node to which name belongs. + * \param[in] node_namespace The namespace of a node to which name belongs. + * \param[in] allocator A valid allocator to use. + * \param[out] output_name Either an allocated string with the remapped name, or + * `NULL` if no remap rules matched the name. + * \return #RCL_RET_OK if the name was remapped or no rules matched, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_SERVICE_NAME_INVALID if the given name is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_remap_service_name( + const rcl_arguments_t * local_arguments, + const rcl_arguments_t * global_arguments, + const char * service_name, + const char * node_name, + const char * node_namespace, + rcl_allocator_t allocator, + char ** output_name); + +/// Remap a node name based on given rules. +/** + * This function returns the node name that a node with the given name would be remapped to. + * When a node's name is remapped it changes its logger name and the output of expanding relative + * topic and service names. + * + * When composing nodes make sure that the final node names used are unique per process. + * There is not currently a way to independently remap the names of two nodes that were created + * with the same node name and are manually composed into one process. + * + * The behavior of `local_arguments`, `global_arguments`, `node_name`, the order remap rules are + * applied, and node specific rules is identical to rcl_remap_topic_name(). + * \sa rcl_remap_topic_name() + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] local_arguments Arguments to be used before global arguments. + * \param[in] global_arguments Command line arguments to use if no local rules matched, or + * `NULL` or zero-initialized to ignore global arguments. + * \param[in] node_name The current name of the node. + * \param[in] allocator A valid allocator to use. + * \param[out] output_name Either an allocated string with the remapped name, or + * `NULL` if no remap rules matched the name. + * \return #RCL_RET_OK If the name was remapped or no rules matched, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_NODE_INVALID_NAME if the name is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_remap_node_name( + const rcl_arguments_t * local_arguments, + const rcl_arguments_t * global_arguments, + const char * node_name, + rcl_allocator_t allocator, + char ** output_name); + +/// Remap a namespace based on given rules. +/** + * This function returns the namespace that a node with the given name would be remapped to. + * When a node's namespace is remapped it changes its logger name and the output of expanding + * relative topic and service names. + * + * The behavior of `local_arguments`, `global_arguments`, `node_name`, the order remap rules are + * applied, and node specific rules is identical to rcl_remap_topic_name(). + * \sa rcl_remap_topic_name() + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] local_arguments Arguments to be used before global arguments. + * \param[in] global_arguments Command line arguments to use if no local rules matched, or + * `NULL` or zero-initialized to ignore global arguments. + * \param[in] node_name The name of the node whose namespace is being remapped. + * \param[in] allocator A valid allocator to be used. + * \param[out] output_namespace Either an allocated string with the remapped namespace, or + * `NULL` if no remap rules matched the name. + * \return #RCL_RET_OK if the node name was remapped or no rules matched, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_NODE_INVALID_NAMESPACE if the remapped namespace is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_remap_node_namespace( + const rcl_arguments_t * local_arguments, + const rcl_arguments_t * global_arguments, + const char * node_name, + rcl_allocator_t allocator, + char ** output_namespace); + +/// Copy one remap structure into another. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] rule The structure to be copied. + * Its allocator is used to copy memory into the new structure. + * \param[out] rule_out A zero-initialized rcl_remap_t structure to be copied into. + * \return #RCL_RET_OK if the structure was copied successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_remap_copy( + const rcl_remap_t * rule, + rcl_remap_t * rule_out); + +/// Reclaim resources held inside rcl_remap_t structure. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] remap The structure to be deallocated. + * \return #RCL_RET_OK if the memory was successfully freed, or + * \return #RCL_RET_INVALID_ARGUMENT if any function arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_remap_fini( + rcl_remap_t * remap); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__REMAP_H_ diff --git a/include/rcl/rmw_implementation_identifier_check.h b/include/rcl/rmw_implementation_identifier_check.h new file mode 100644 index 0000000..cf39fc0 --- /dev/null +++ b/include/rcl/rmw_implementation_identifier_check.h @@ -0,0 +1,48 @@ +// 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. + +/// @file + +#ifndef RCL__RMW_IMPLEMENTATION_IDENTIFIER_CHECK_H_ +#define RCL__RMW_IMPLEMENTATION_IDENTIFIER_CHECK_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/visibility_control.h" + +/// The environment variable name to control which RMW implementation is used. +#define RMW_IMPLEMENTATION_ENV_VAR_NAME "RMW_IMPLEMENTATION" + +/// The environment variable name to control whether the chosen RMW implementation +/// matches the one that is in use. +#define RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME "RCL_ASSERT_RMW_ID_MATCHES" + +/// Check whether the RMW implementation in use matches what the user requested. +/** + * \return #RCL_RET_OK if the RMW implementation in use matches what the user requested, or + * \return #RCL_RET_MISMATCHED_RMW_ID if the RMW implementation does not match, or + * \return #RCL_RET_BAD_ALLOC if memory allocation failed, or + * \return #RCL_RET_ERROR if some other error occurred. + */ +RCL_PUBLIC +rcl_ret_t rcl_rmw_implementation_identifier_check(void); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__RMW_IMPLEMENTATION_IDENTIFIER_CHECK_H_ diff --git a/include/rcl/security.h b/include/rcl/security.h new file mode 100644 index 0000000..b37fd92 --- /dev/null +++ b/include/rcl/security.h @@ -0,0 +1,133 @@ +// Copyright 2018-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. + +/// @file + +#ifndef RCL__SECURITY_H_ +#define RCL__SECURITY_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +#include "rcl/allocator.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" +#include "rmw/security_options.h" + +#ifndef ROS_SECURITY_ENCLAVE_OVERRIDE +/// The name of the environment variable containing the security enclave override. +# define ROS_SECURITY_ENCLAVE_OVERRIDE "ROS_SECURITY_ENCLAVE_OVERRIDE" +#endif + +#ifndef ROS_SECURITY_KEYSTORE_VAR_NAME +/// The name of the environment variable containing the path to the keystore. +# define ROS_SECURITY_KEYSTORE_VAR_NAME "ROS_SECURITY_KEYSTORE" +#endif + +#ifndef ROS_SECURITY_STRATEGY_VAR_NAME +/// The name of the environment variable containing the security strategy. +# define ROS_SECURITY_STRATEGY_VAR_NAME "ROS_SECURITY_STRATEGY" +#endif + +#ifndef ROS_SECURITY_ENABLE_VAR_NAME +/// The name of the environment variable controlling whether security is enabled. +# define ROS_SECURITY_ENABLE_VAR_NAME "ROS_SECURITY_ENABLE" +#endif + +/// Initialize security options from values in the environment variables and given names. +/** + * Initialize the given security options based on the environment. + * For more details: + * \sa rcl_security_enabled() + * \sa rcl_get_enforcement_policy() + * \sa rcl_get_secure_root() + * + * \param[in] name name used to find the security root path. + * \param[in] allocator used to do allocations. + * \param[out] security_options security options that will be configured according to + * the environment. + * \return #RCL_RET_OK If the security options are returned properly, or + * \return #RCL_RET_INVALID_ARGUMENT if an argument is not valid, or + * \return #RCL_RET_ERROR if an unexpected error happened + */ +RCL_PUBLIC +rcl_ret_t +rcl_get_security_options_from_environment( + const char * name, + const rcutils_allocator_t * allocator, + rmw_security_options_t * security_options); + +/// Check if security has to be used, according to the environment. +/** + * If the `ROS_SECURITY_ENABLE` environment variable is set to "true", `use_security` will be set to + * true. + * + * \param[out] use_security Must not be NULL. + * \return #RCL_RET_INVALID_ARGUMENT if an argument is not valid, or + * \return #RCL_RET_ERROR if an unexpected error happened, or + * \return #RCL_RET_OK. + */ +RCL_PUBLIC +rcl_ret_t +rcl_security_enabled(bool * use_security); + +/// Get security enforcement policy from the environment. +/** + * Sets `policy` based on the value of the `ROS_SECURITY_STRATEGY` environment variable. + * If `ROS_SECURITY_STRATEGY` is "Enforce", `policy` will be `RMW_SECURITY_ENFORCEMENT_ENFORCE`. + * If not, `policy` will be `RMW_SECURITY_ENFORCEMENT_PERMISSIVE`. + * + * \param[out] policy Must not be NULL. + * \return #RCL_RET_INVALID_ARGUMENT if an argument is not valid, or + * \return #RCL_RET_ERROR if an unexpected error happened, or + * \return #RCL_RET_OK. + */ +RCL_PUBLIC +rcl_ret_t +rcl_get_enforcement_policy(rmw_security_enforcement_policy_t * policy); + +/// Return the secure root given a enclave name. +/** + * Return the security directory associated with the enclave name. + * + * The value of the environment variable `ROS_SECURITY_KEYSTORE` is used as a root. + * The specific directory to be used is found from that root using the `name` passed. + * E.g. for a context named "/a/b/c" and root "/r", the secure root path will be + * "/r/a/b/c", where the delimiter "/" is native for target file system (e.g. "\\" for _WIN32). + * + * However, this expansion can be overridden by setting the secure enclave override environment + * (`ROS_SECURITY_ENCLAVE_OVERRIDE`) variable, allowing users to explicitly specify the exact enclave + * `name` to be utilized. + * Such an override is useful for applications where the enclave is non-deterministic + * before runtime, or when testing and using additional tools that may not otherwise be easily + * provisioned. + * + * \param[in] name validated name (a single token) + * \param[in] allocator the allocator to use for allocation + * \return Machine specific (absolute) enclave directory path or NULL on failure. + * Returned pointer must be deallocated by the caller of this function + */ +RCL_PUBLIC +char * +rcl_get_secure_root(const char * name, const rcl_allocator_t * allocator); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__SECURITY_H_ diff --git a/include/rcl/service.h b/include/rcl/service.h new file mode 100644 index 0000000..2461bd5 --- /dev/null +++ b/include/rcl/service.h @@ -0,0 +1,531 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file + +#ifndef RCL__SERVICE_H_ +#define RCL__SERVICE_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rosidl_runtime_c/service_type_support_struct.h" + +#include "rcl/event_callback.h" +#include "rcl/macros.h" +#include "rcl/node.h" +#include "rcl/visibility_control.h" + +/// Internal rcl implementation struct. +typedef struct rcl_service_impl_s rcl_service_impl_t; + +/// Structure which encapsulates a ROS Service. +typedef struct rcl_service_s +{ + /// Pointer to the service implementation + rcl_service_impl_t * impl; +} rcl_service_t; + +/// Options available for a rcl service. +typedef struct rcl_service_options_s +{ + /// Middleware quality of service settings for the service. + rmw_qos_profile_t qos; + /// Custom allocator for the service, used for incidental allocations. + /** For default behavior (malloc/free), see: rcl_get_default_allocator() */ + rcl_allocator_t allocator; +} rcl_service_options_t; + +/// Return a rcl_service_t struct with members set to `NULL`. +/** + * Should be called to get a null rcl_service_t before passing to + * rcl_service_init(). + * + * \return A structure with a zero initialized service. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_service_t +rcl_get_zero_initialized_service(void); + +/// Initialize a rcl service. +/** + * After calling this function on a rcl_service_t, it can be used to take + * requests of the given type to the given topic using rcl_take_request(). + * It can also send a response to a request using rcl_send_response(). + * + * The given rcl_node_t must be valid and the resulting rcl_service_t is + * only valid as long as the given rcl_node_t remains valid. + * + * The rosidl_service_type_support_t is obtained on a per .srv type basis. + * When the user defines a ROS service, code is generated which provides the + * required rosidl_service_type_support_t object. + * This object can be obtained using a language appropriate mechanism. + * \todo TODO(wjwwood) write these instructions once and link to it instead + * + * For C, a macro can be used (for example `example_interfaces/AddTwoInts`): + * + * ```c + * #include + * #include + * const rosidl_service_type_support_t * ts = + * ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts); + * ``` + * + * For C++, a template function is used: + * + * ```cpp + * #include + * #include + * using rosidl_typesupport_cpp::get_service_type_support_handle; + * const rosidl_service_type_support_t * ts = + * get_service_type_support_handle(); + * ``` + * + * The rosidl_service_type_support_t object contains service type specific + * information used to send or take requests and responses. + * + * The topic name must be a c string which follows the topic and service name + * format rules for unexpanded names, also known as non-fully qualified names: + * + * \see rcl_expand_topic_name + * + * The options struct allows the user to set the quality of service settings as + * well as a custom allocator which is used when initializing/finalizing the + * client to allocate space for incidentals, e.g. the service name string. + * + * Expected usage (for C services): + * + * ```c + * #include + * #include + * #include + * + * rcl_node_t node = rcl_get_zero_initialized_node(); + * rcl_node_options_t node_ops = rcl_node_get_default_options(); + * rcl_ret_t ret = rcl_node_init(&node, "node_name", "/my_namespace", &node_ops); + * // ... error handling + * const rosidl_service_type_support_t * ts = + * ROSIDL_GET_SRV_TYPE_SUPPORT(example_interfaces, srv, AddTwoInts); + * rcl_service_t service = rcl_get_zero_initialized_service(); + * rcl_service_options_t service_ops = rcl_service_get_default_options(); + * ret = rcl_service_init(&service, &node, ts, "add_two_ints", &service_ops); + * // ... error handling, and on shutdown do finalization: + * ret = rcl_service_fini(&service, &node); + * // ... error handling for rcl_service_fini() + * ret = rcl_node_fini(&node); + * // ... error handling for rcl_node_fini() + * ``` + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[out] service preallocated service structure + * \param[in] node valid rcl node handle + * \param[in] type_support type support object for the service's type + * \param[in] service_name the name of the service + * \param[in] options service options, including quality of service settings + * \return #RCL_RET_OK if service was initialized successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ALREADY_INIT if the service is already initialized, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_SERVICE_NAME_INVALID if the given service name is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_init( + rcl_service_t * service, + const rcl_node_t * node, + const rosidl_service_type_support_t * type_support, + const char * service_name, + const rcl_service_options_t * options); + +/// Finalize a rcl_service_t. +/** + * After calling, the node will no longer listen for requests for this service. + * (assuming this is the only service of this type in this node). + * + * After calling, calls to rcl_wait(), rcl_take_request(), and + * rcl_send_response() will fail when using this service. + * Additionally rcl_wait() will be interrupted if currently blocking. + * However, the given node handle is still valid. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] service handle to the service to be deinitialized + * \param[in] node a valid (not finalized) handle to the node used to create the service + * \return #RCL_RET_OK if service was deinitialized successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_SERVICE_INVALID if the service is invalid, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_fini(rcl_service_t * service, rcl_node_t * node); + +/// Return the default service options in a rcl_service_options_t. +/** + * The defaults are: + * + * - qos = rmw_qos_profile_services_default + * - allocator = rcl_get_default_allocator() + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_service_options_t +rcl_service_get_default_options(void); + +/// Take a pending ROS request using a rcl service. +/** + * It is the job of the caller to ensure that the type of the ros_request + * argument and the type associate with the service, via the type + * support, match. + * Passing a different type to rcl_take produces undefined behavior and cannot + * be checked by this function and therefore no deliberate error will occur. + * + * TODO(jacquelinekay) blocking of take? + * TODO(jacquelinekay) pre-, during-, and post-conditions for message ownership? + * TODO(jacquelinekay) is rcl_take_request thread-safe? + * TODO(jacquelinekay) Should there be an rcl_request_id_t? + * + * The ros_request pointer should point to an already allocated ROS request message + * struct of the correct type, into which the taken ROS request will be copied + * if one is available. + * If taken is false after calling, then the ROS request will be unmodified. + * + * If allocation is required when taking the request, e.g. if space needs to + * be allocated for a dynamically sized array in the target message, then the + * allocator given in the service options is used. + * + * request_header is a pointer to pre-allocated a rmw struct containing + * meta-information about the request (e.g. the sequence number). + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Maybe [1] + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * [1] only if required when filling the request, avoided for fixed sizes + * + * \param[in] service the handle to the service from which to take + * \param[inout] request_header ptr to the struct holding metadata about the request + * \param[inout] ros_request type-erased ptr to an allocated ROS request message + * \return #RCL_RET_OK if the request was taken, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_SERVICE_INVALID if the service is invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_SERVICE_TAKE_FAILED if take failed but no error occurred + * in the middleware, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_request_with_info( + const rcl_service_t * service, + rmw_service_info_t * request_header, + void * ros_request); + +/// Backwards compatibility function to take a pending ROS request using a rcl service. +/** + * This version takes a request ID only. See rcl_take_request_with_info() for a full + * explanation of what this does. + * + * \param[in] service the handle to the service from which to take + * \param[inout] request_header ptr to the struct holding the id of the request + * \param[inout] ros_request type-erased ptr to an allocated ROS request message + * \return #RCL_RET_OK if the request was taken, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_SERVICE_INVALID if the service is invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_SERVICE_TAKE_FAILED if take failed but no error occurred + * in the middleware, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_request( + const rcl_service_t * service, + rmw_request_id_t * request_header, + void * ros_request); + +/// Send a ROS response to a client using a service. +/** + * It is the job of the caller to ensure that the type of the `ros_response` + * parameter and the type associate with the service (via the type support) + * match. + * Passing a different type to send_response produces undefined behavior and + * cannot be checked by this function and therefore no deliberate error will + * occur. + * + * send_response() is an non-blocking call. + * + * The ROS response message given by the `ros_response` void pointer is always + * owned by the calling code, but should remain constant during + * rcl_send_response(). + * + * This function is thread safe so long as access to both the service and the + * `ros_response` is synchronized. + * That means that calling rcl_send_response() from multiple threads is + * allowed, but calling rcl_send_response() at the same time as non-thread safe + * service functions is not, e.g. calling rcl_send_response() and + * rcl_service_fini() concurrently is not allowed. + * Before calling rcl_send_response() the message can change and after calling + * rcl_send_response() the message can change, but it cannot be changed during + * the rcl_send_response() call. + * The same `ros_response`, however, can be passed to multiple calls of + * rcl_send_response() simultaneously, even if the services differ. + * The `ros_response` is unmodified by rcl_send_response(). + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes [1] + * Uses Atomics | No + * Lock-Free | Yes + * [1] for unique pairs of services and responses, see above for more + * + * \param[in] service handle to the service which will make the response + * \param[inout] response_header ptr to the struct holding metadata about the request ID + * \param[in] ros_response type-erased pointer to the ROS response message + * \return #RCL_RET_OK if the response was sent successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_SERVICE_INVALID if the service is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_send_response( + const rcl_service_t * service, + rmw_request_id_t * response_header, + void * ros_response); + +/// Get the topic name for the service. +/** + * This function returns the service's internal topic name string. + * This function can fail, and therefore return `NULL`, if the: + * - service is `NULL` + * - service is invalid (never called init, called fini, or invalid) + * + * The returned string is only valid as long as the service is valid. + * The value of the string may change if the topic name changes, and therefore + * copying the string is recommended if this is a concern. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] service the pointer to the service + * \return name string if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_service_get_service_name(const rcl_service_t * service); + +/// Return the rcl service options. +/** + * This function returns the service's internal options struct. + * This function can fail, and therefore return `NULL`, if the: + * - service is `NULL` + * - service is invalid (never called init, called fini, or invalid) + * + * The returned struct is only valid as long as the service is valid. + * The values in the struct may change if the service's options change, + * and therefore copying the struct is recommended if this is a concern. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] service pointer to the service + * \return options struct if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rcl_service_options_t * +rcl_service_get_options(const rcl_service_t * service); + +/// Return the rmw service handle. +/** + * The handle returned is a pointer to the internally held rmw handle. + * This function can fail, and therefore return `NULL`, if the: + * - service is `NULL` + * - service is invalid (never called init, called fini, or invalid) + * + * The returned handle is made invalid if the service is finalized or if + * rcl_shutdown() is called. + * The returned handle is not guaranteed to be valid for the life time of the + * service as it may be finalized and recreated itself. + * Therefore it is recommended to get the handle from the service using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] service pointer to the rcl service + * \return rmw service handle if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_service_t * +rcl_service_get_rmw_handle(const rcl_service_t * service); + +/// Check that the service is valid. +/** + * The bool returned is `false` if `service` is invalid. + * The bool returned is `true` otherwise. + * In the case where `false` is to be returned, an error message is set. + * This function cannot fail. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] service pointer to the rcl service + * \return `true` if `service` is valid, otherwise `false` + */ +RCL_PUBLIC +bool +rcl_service_is_valid(const rcl_service_t * service); + +/// Get the actual qos settings of the service's request subscription. +/** + * Used to get the actual qos settings of the service's request subscription. + * The actual configuration applied when using RMW_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the service, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_*_UNKNOWN. + * The returned struct is only valid as long as the rcl_service_t is valid. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] service pointer to the rcl service + * \return qos struct if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rmw_qos_profile_t * +rcl_service_request_subscription_get_actual_qos(const rcl_service_t * service); + +/// Get the actual qos settings of the service's response publisher. +/** + * Used to get the actual qos settings of the service's response publisher. + * The actual configuration applied when using RMW_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the service, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_*_UNKNOWN. + * The returned struct is only valid as long as the rcl_service_t is valid. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] service pointer to the rcl service + * \return qos struct if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rmw_qos_profile_t * +rcl_service_response_publisher_get_actual_qos(const rcl_service_t * service); + +/// Set the on new request callback function for the service. +/** + * This API sets the callback function to be called whenever the + * service is notified about a new request. + * + * \sa rmw_service_set_on_new_request_callback for details about this function. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] rmw implementation defined + * + * \param[in] service The service on which to set the callback + * \param[in] callback The callback to be called when new requests arrive, may be NULL + * \param[in] user_data Given to the callback when called later, may be NULL + * \return `RCL_RET_OK` if callback was set to the listener, or + * \return `RCL_RET_INVALID_ARGUMENT` if `service` is NULL, or + * \return `RCL_RET_UNSUPPORTED` if the API is not implemented in the dds implementation + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_service_set_on_new_request_callback( + const rcl_service_t * service, + rcl_event_callback_t callback, + const void * user_data); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__SERVICE_H_ diff --git a/include/rcl/subscription.h b/include/rcl/subscription.h new file mode 100644 index 0000000..28bbf17 --- /dev/null +++ b/include/rcl/subscription.h @@ -0,0 +1,875 @@ +// Copyright 2015 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. + +/// @file + +#ifndef RCL__SUBSCRIPTION_H_ +#define RCL__SUBSCRIPTION_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rosidl_runtime_c/message_type_support_struct.h" + +#include "rcl/event_callback.h" +#include "rcl/macros.h" +#include "rcl/node.h" +#include "rcl/visibility_control.h" + +#include "rmw/message_sequence.h" + +/// Internal rcl implementation struct. +typedef struct rcl_subscription_impl_s rcl_subscription_impl_t; + +/// Structure which encapsulates a ROS Subscription. +typedef struct rcl_subscription_s +{ + /// Pointer to the subscription implementation + rcl_subscription_impl_t * impl; +} rcl_subscription_t; + +/// Options available for a rcl subscription. +typedef struct rcl_subscription_options_s +{ + /// Middleware quality of service settings for the subscription. + rmw_qos_profile_t qos; + /// Custom allocator for the subscription, used for incidental allocations. + /** For default behavior (malloc/free), see: rcl_get_default_allocator() */ + rcl_allocator_t allocator; + /// rmw specific subscription options, e.g. the rmw implementation specific payload. + rmw_subscription_options_t rmw_subscription_options; +} rcl_subscription_options_t; + +typedef struct rcl_subscription_content_filter_options_s +{ + rmw_subscription_content_filter_options_t rmw_subscription_content_filter_options; +} rcl_subscription_content_filter_options_t; + +/// Return a rcl_subscription_t struct with members set to `NULL`. +/** + * Should be called to get a null rcl_subscription_t before passing to + * rcl_subscription_init(). + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_subscription_t +rcl_get_zero_initialized_subscription(void); + +/// Initialize a ROS subscription. +/** + * After calling this function on a rcl_subscription_t, it can be used to take + * messages of the given type to the given topic using rcl_take(). + * + * The given rcl_node_t must be valid and the resulting rcl_subscription_t is + * only valid as long as the given rcl_node_t remains valid. + * + * The rosidl_message_type_support_t is obtained on a per .msg type basis. + * When the user defines a ROS message, code is generated which provides the + * required rosidl_message_type_support_t object. + * This object can be obtained using a language appropriate mechanism. + * \todo TODO(wjwwood) write these instructions once and link to it instead + * For C a macro can be used (for example `std_msgs/String`): + * + * ```c + * #include + * #include + * const rosidl_message_type_support_t * string_ts = + * ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String); + * ``` + * + * For C++ a template function is used: + * + * ```cpp + * #include + * #include + * using rosidl_typesupport_cpp::get_message_type_support_handle; + * const rosidl_message_type_support_t * string_ts = + * get_message_type_support_handle(); + * ``` + * + * The rosidl_message_type_support_t object contains message type specific + * information used to publish messages. + * + * The topic name must be a c string which follows the topic and service name + * format rules for unexpanded names, also known as non-fully qualified names: + * + * \see rcl_expand_topic_name + * + * The options struct allows the user to set the quality of service settings as + * well as a custom allocator which is used when (de)initializing the + * subscription to allocate space for incidental things, e.g. the topic + * name string. + * + * Expected usage (for C messages): + * + * ```c + * #include + * #include + * #include + * + * rcl_node_t node = rcl_get_zero_initialized_node(); + * rcl_node_options_t node_ops = rcl_node_get_default_options(); + * rcl_ret_t ret = rcl_node_init(&node, "node_name", "/my_namespace", &node_ops); + * // ... error handling + * const rosidl_message_type_support_t * ts = + * ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String); + * rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + * rcl_subscription_options_t subscription_ops = rcl_subscription_get_default_options(); + * ret = rcl_subscription_init(&subscription, &node, ts, "chatter", &subscription_ops); + * // ... error handling, and when finished deinitialization + * ret = rcl_subscription_fini(&subscription, &node); + * // ... error handling for rcl_subscription_fini() + * ret = rcl_node_fini(&node); + * // ... error handling for rcl_node_fini() + * ``` + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[out] subscription preallocated subscription structure + * \param[in] node valid rcl node handle + * \param[in] type_support type support object for the topic's type + * \param[in] topic_name the name of the topic + * \param[in] options subscription options, including quality of service settings + * \return #RCL_RET_OK if subscription was initialized successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ALREADY_INIT if the subcription is already initialized, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_TOPIC_NAME_INVALID if the given topic name is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_init( + rcl_subscription_t * subscription, + const rcl_node_t * node, + const rosidl_message_type_support_t * type_support, + const char * topic_name, + const rcl_subscription_options_t * options +); + +/// Finalize a rcl_subscription_t. +/** + * After calling, the node will no longer be subscribed on this topic + * (assuming this is the only subscription on this topic in this node). + * + * After calling, calls to rcl_wait and rcl_take will fail when using this + * subscription. + * Additioanlly rcl_wait will be interrupted if currently blocking. + * However, the given node handle is still valid. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] subscription handle to the subscription to be deinitialized + * \param[in] node a valid (not finalized) handle to the node used to create the subscription + * \return #RCL_RET_OK if subscription was deinitialized successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or + * \return #RCL_RET_NODE_INVALID if the node is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node); + +/// Return the default subscription options in a rcl_subscription_options_t. +/** + * The defaults are: + * + * - qos = rmw_qos_profile_default + * - allocator = rcl_get_default_allocator() + * - rmw_subscription_options = rmw_get_default_subscription_options(); + * + * \return A structure containing the default options for a subscription. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_subscription_options_t +rcl_subscription_get_default_options(void); + +/// Reclaim resources held inside rcl_subscription_options_t structure. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] option The structure which its resources have to be deallocated. + * \return `RCL_RET_OK` if the memory was successfully freed, or + * \return `RCL_RET_INVALID_ARGUMENT` if option is NULL, or + * \return `RCL_RET_BAD_ALLOC` if deallocating memory fails. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_options_fini(rcl_subscription_options_t * option); + +/// Set the content filter options for the given subscription options. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] filter_expression The filter expression is similar to the WHERE part of an SQL clause. + * \param[in] expression_parameters_argc The maximum of expression parameters argc is 100. + * \param[in] expression_parameter_argv The expression parameters argv are the tokens placeholder + * ‘parameters’ (i.e., "%n" tokens begin from 0) in the filter_expression. + * + * It can be NULL if there is no "%n" tokens placeholder in filter_expression. + * \param[out] options The subscription options to be set. + * \return `RCL_RET_OK` if set options successfully, or + * \return `RCL_RET_INVALID_ARGUMENT` if arguments invalid, or + * \return `RCL_RET_BAD_ALLOC` if allocating memory fails. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_options_set_content_filter_options( + const char * filter_expression, + size_t expression_parameters_argc, + const char * expression_parameter_argv[], + rcl_subscription_options_t * options); + +/// Return the zero initialized subscription content filter options. +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_subscription_content_filter_options_t +rcl_get_zero_initialized_subscription_content_filter_options(void); + +/// Initialize the content filter options for the given subscription options. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] subscription the handle to the subscription. + * \param[in] filter_expression The filter expression is similar to the WHERE part of an SQL clause, + * use empty ("") can reset (or clear) the content filter setting of a subscription. + * \param[in] expression_parameters_argc The maximum of expression parameters argc is 100. + * \param[in] expression_parameter_argv The expression parameters argv are the tokens placeholder + * ‘parameters’ (i.e., "%n" tokens begin from 0) in the filter_expression. + * + * It can be NULL if there is no "%n" tokens placeholder in filter_expression. + * \param[out] options The subscription options to be set. + * \return `RCL_RET_OK` if set options successfully, or + * \return `RCL_RET_SUBSCRIPTION_INVALID` if subscription is invalid, or + * \return `RCL_RET_INVALID_ARGUMENT` if arguments invalid, or + * \return `RCL_RET_BAD_ALLOC` if allocating memory fails. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_content_filter_options_init( + const rcl_subscription_t * subscription, + const char * filter_expression, + size_t expression_parameters_argc, + const char * expression_parameter_argv[], + rcl_subscription_content_filter_options_t * options); + +/// Set the content filter options for the given subscription options. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] subscription the handle to the subscription. + * \param[in] filter_expression The filter expression is similar to the WHERE part of an SQL clause, + * use empty ("") can reset (or clear) the content filter setting of a subscription. + * \param[in] expression_parameters_argc The maximum of expression parameters argc is 100. + * \param[in] expression_parameter_argv The expression parameters argv are the tokens placeholder + * ‘parameters’ (i.e., "%n" tokens begin from 0) in the filter_expression. + * + * It can be NULL if there is no "%n" tokens placeholder in filter_expression. + * \param[out] options The subscription options to be set. + * \return `RCL_RET_OK` if set options successfully, or + * \return `RCL_RET_SUBSCRIPTION_INVALID` if subscription is invalid, or + * \return `RCL_RET_INVALID_ARGUMENT` if arguments invalid, or + * \return `RCL_RET_BAD_ALLOC` if allocating memory fails. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_content_filter_options_set( + const rcl_subscription_t * subscription, + const char * filter_expression, + size_t expression_parameters_argc, + const char * expression_parameter_argv[], + rcl_subscription_content_filter_options_t * options); + +/// Reclaim rcl_subscription_content_filter_options_t structure. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | No + * + * \param[in] subscription the handle to the subscription. + * \param[in] options The structure which its resources have to be deallocated. + * \return `RCL_RET_OK` if the memory was successfully freed, or + * \return `RCL_RET_SUBSCRIPTION_INVALID` if subscription is invalid, or + * \return `RCL_RET_INVALID_ARGUMENT` if option is NULL, or + * if its allocator is invalid and the structure contains initialized memory. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_content_filter_options_fini( + const rcl_subscription_t * subscription, + rcl_subscription_content_filter_options_t * options); + +/// Check if the content filtered topic feature is enabled in the subscription. +/** + * Depending on the middleware and whether cft is enabled in the subscription. + * + * \return `true` if the content filtered topic of `subscription` is enabled, otherwise `false` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +bool +rcl_subscription_is_cft_enabled(const rcl_subscription_t * subscription); + +/// Set the filter expression and expression parameters for the subscription. +/** + * This function will set a filter expression and an array of expression parameters + * for the given subscription. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * + * \param[in] subscription The subscription to set content filter options. + * \param[in] options The rcl content filter options. + * \return `RCL_RET_OK` if the query was successful, or + * \return `RCL_RET_INVALID_ARGUMENT` if `subscription` is NULL, or + * \return `RCL_RET_INVALID_ARGUMENT` if `options` is NULL, or + * \return `RCL_RET_UNSUPPORTED` if the implementation does not support content filter topic, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_set_content_filter( + const rcl_subscription_t * subscription, + const rcl_subscription_content_filter_options_t * options +); + +/// Retrieve the filter expression of the subscription. +/** + * This function will return an filter expression by the given subscription. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * + * \param[in] subscription The subscription object to inspect. + * \param[out] options The rcl content filter options. + * It is up to the caller to finalize this options later on, using + * rcl_subscription_content_filter_options_fini(). + * \return `RCL_RET_OK` if the query was successful, or + * \return `RCL_RET_INVALID_ARGUMENT` if `subscription` is NULL, or + * \return `RCL_RET_INVALID_ARGUMENT` if `options` is NULL, or + * \return `RCL_RET_BAD_ALLOC` if memory allocation fails, or + * \return `RCL_RET_UNSUPPORTED` if the implementation does not support content filter topic, or + * \return `RCL_RET_ERROR` if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_get_content_filter( + const rcl_subscription_t * subscription, + rcl_subscription_content_filter_options_t * options +); + +/// Take a ROS message from a topic using a rcl subscription. +/** + * It is the job of the caller to ensure that the type of the ros_message + * argument and the type associated with the subscription, via the type + * support, match. + * Passing a different type to rcl_take produces undefined behavior and cannot + * be checked by this function and therefore no deliberate error will occur. + * + * TODO(wjwwood) blocking of take? + * TODO(wjwwood) pre-, during-, and post-conditions for message ownership? + * TODO(wjwwood) is rcl_take thread-safe? + * TODO(wjwwood) Should there be an rcl_message_info_t? + * + * The ros_message pointer should point to an already allocated ROS message + * struct of the correct type, into which the taken ROS message will be copied + * if one is available. + * If taken is false after calling, then the ROS message will be unmodified. + * + * The taken boolean may be false even if a wait set reports that the + * subscription was ready to be taken from in some cases, e.g. when the + * state of the subscription changes it may cause the wait set to wake up + * but subsequent takes to fail to take anything. + * + * If allocation is required when taking the message, e.g. if space needs to + * be allocated for a dynamically sized array in the target message, then the + * allocator given in the subscription options is used. + * + * The rmw_message_info struct contains meta information about this particular + * message instance, like what the GUID of the publisher which published it + * originally or whether or not the message received from within the same + * process. + * The message_info argument should be an already allocated rmw_message_info_t + * structure. + * Passing `NULL` for message_info will result in the argument being ignored. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Maybe [1] + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * [1] only if required when filling the message, avoided for fixed sizes + * + * \param[in] subscription the handle to the subscription from which to take + * \param[inout] ros_message type-erased ptr to a allocated ROS message + * \param[out] message_info rmw struct which contains meta-data for the message + * \param[in] allocation structure pointer used for memory preallocation (may be NULL) + * \return #RCL_RET_OK if the message was taken, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_SUBSCRIPTION_TAKE_FAILED if take failed but no error + * occurred in the middleware, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take( + const rcl_subscription_t * subscription, + void * ros_message, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation +); + +/// Take a sequence of messages from a topic using a rcl subscription. +/** + * In contrast to rcl_take(), this function can take multiple messages at + * the same time. + * It is the job of the caller to ensure that the type of the message_sequence + * argument and the type associated with the subscription, via the type + * support, match. + * + * The message_sequence pointer should point to an already allocated sequence + * of ROS messages of the correct type, into which the taken ROS messages will + * be copied if messages are available. + * The message_sequence `size` member will be set to the number of messages + * correctly taken. + * + * The rmw_message_info_sequence struct contains meta information about the + * corresponding message instance index. + * The message_info_sequence argument should be an already allocated + * rmw_message_info_sequence_t structure. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Maybe [1] + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * [1] only if storage in the serialized_message is insufficient + * + * \param[in] subscription the handle to the subscription from which to take. + * \param[in] count number of messages to attempt to take. + * \param[inout] message_sequence pointer to a (pre-allocated) message sequence. + * \param[inout] message_info_sequence pointer to a (pre-allocated) message info sequence. + * \param[in] allocation structure pointer used for memory preallocation (may be NULL) + * \return #RCL_RET_OK if one or more messages was taken, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_SUBSCRIPTION_TAKE_FAILED if take failed but no error + * occurred in the middleware, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_sequence( + const rcl_subscription_t * subscription, + size_t count, + rmw_message_sequence_t * message_sequence, + rmw_message_info_sequence_t * message_info_sequence, + rmw_subscription_allocation_t * allocation +); + +/// Take a serialized raw message from a topic using a rcl subscription. +/** + * In contrast to rcl_take(), this function stores the taken message in + * its raw binary representation. + * It is the job of the caller to ensure that the type associate with the subscription + * matches, and can optionally be deserialized into its ROS message via, the correct + * type support. + * If the `serialized_message` parameter contains enough preallocated memory, the incoming + * message can be taken without any additional memory allocation. + * If not, the function will dynamically allocate enough memory for the message. + * Passing a different type to rcl_take produces undefined behavior and cannot + * be checked by this function and therefore no deliberate error will occur. + * + * Apart from the differences above, this function behaves like rcl_take(). + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Maybe [1] + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * [1] only if storage in the serialized_message is insufficient + * + * \param[in] subscription the handle to the subscription from which to take + * \param[inout] serialized_message pointer to a (pre-allocated) serialized message. + * \param[out] message_info rmw struct which contains meta-data for the message + * \param[in] allocation structure pointer used for memory preallocation (may be NULL) + * \return #RCL_RET_OK if the message was published, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_SUBSCRIPTION_TAKE_FAILED if take failed but no error + * occurred in the middleware, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_serialized_message( + const rcl_subscription_t * subscription, + rcl_serialized_message_t * serialized_message, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation); + +/// Take a loaned message from a topic using a rcl subscription. +/** + * Depending on the middleware, incoming messages can be loaned to the user's callback + * without further copying. + * The implicit contract here is that the middleware owns the memory allocated for this message. + * The user must not destroy the message, but rather has to return it with a call to + * \sa rcl_return_loaned_message to the middleware. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] subscription the handle to the subscription from which to take + * \param[inout] loaned_message a pointer to the loaned messages. + * \param[out] message_info rmw struct which contains meta-data for the message. + * \param[in] allocation structure pointer used for memory preallocation (may be NULL) + * \return #RCL_RET_OK if the loaned message sequence was taken, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_SUBSCRIPTION_TAKE_FAILED if take failed but no error + * occurred in the middleware, or + * \return #RCL_RET_UNSUPPORTED if the middleware does not support that feature, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_take_loaned_message( + const rcl_subscription_t * subscription, + void ** loaned_message, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation); + +/// Return a loaned message from a topic using a rcl subscription. +/** + * If a loaned message was previously obtained from the middleware with a call to + * \sa rcl_take_loaned_message, this message has to be returned to indicate to the middleware + * that the user no longer needs that memory. + * The user must not delete the message. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] subscription the handle to the subscription from which to take + * \param[in] loaned_message a pointer to the loaned messages. + * \return #RCL_RET_OK if the message was published, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or + * \return #RCL_RET_UNSUPPORTED if the middleware does not support that feature, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_return_loaned_message_from_subscription( + const rcl_subscription_t * subscription, + void * loaned_message); + +/// Get the topic name for the subscription. +/** + * This function returns the subscription's internal topic name string. + * This function can fail, and therefore return `NULL`, if the: + * - subscription is `NULL` + * - subscription is invalid (never called init, called fini, or invalid) + * + * The returned string is only valid as long as the subscription is valid. + * The value of the string may change if the topic name changes, and therefore + * copying the string is recommended if this is a concern. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] subscription the pointer to the subscription + * \return name string if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_subscription_get_topic_name(const rcl_subscription_t * subscription); + +/// Return the rcl subscription options. +/** + * This function returns the subscription's internal options struct. + * This function can fail, and therefore return `NULL`, if the: + * - subscription is `NULL` + * - subscription is invalid (never called init, called fini, or invalid) + * + * The returned struct is only valid as long as the subscription is valid. + * The values in the struct may change if the subscription's options change, + * and therefore copying the struct is recommended if this is a concern. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] subscription pointer to the subscription + * \return options struct if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rcl_subscription_options_t * +rcl_subscription_get_options(const rcl_subscription_t * subscription); + +/// Return the rmw subscription handle. +/** + * The handle returned is a pointer to the internally held rmw handle. + * This function can fail, and therefore return `NULL`, if the: + * - subscription is `NULL` + * - subscription is invalid (never called init, called fini, or invalid) + * + * The returned handle is made invalid if the subscription is finalized or if + * rcl_shutdown() is called. + * The returned handle is not guaranteed to be valid for the life time of the + * subscription as it may be finalized and recreated itself. + * Therefore it is recommended to get the handle from the subscription using + * this function each time it is needed and avoid use of the handle + * concurrently with functions that might change it. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] subscription pointer to the rcl subscription + * \return rmw subscription handle if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_subscription_t * +rcl_subscription_get_rmw_handle(const rcl_subscription_t * subscription); + +/// Check that the subscription is valid. +/** + * The bool returned is `false` if `subscription` is invalid. + * The bool returned is `true` otherwise. + * In the case where `false` is to be returned, an error message is set. + * This function cannot fail. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] subscription pointer to the rcl subscription + * \return `true` if `subscription` is valid, otherwise `false` + */ +RCL_PUBLIC +bool +rcl_subscription_is_valid(const rcl_subscription_t * subscription); + +/// Get the number of publishers matched to a subscription. +/** + * Used to get the internal count of publishers matched to a subscription. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] only if the underlying rmw doesn't make use of this feature + * + * \param[in] subscription pointer to the rcl subscription + * \param[out] publisher_count number of matched publishers + * \return #RCL_RET_OK if the count was retrieved, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_SUBSCRIPTION_INVALID if the subscription is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rmw_ret_t +rcl_subscription_get_publisher_count( + const rcl_subscription_t * subscription, + size_t * publisher_count); + +/// Get the actual qos settings of the subscription. +/** + * Used to get the actual qos settings of the subscription. + * The actual configuration applied when using RMW_*_SYSTEM_DEFAULT + * can only be resolved after the creation of the subscription, and it + * depends on the underlying rmw implementation. + * If the underlying setting in use can't be represented in ROS terms, + * it will be set to RMW_*_UNKNOWN. + * The returned struct is only valid as long as the rcl_subscription_t is valid. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] subscription pointer to the rcl subscription + * \return qos struct if successful, otherwise `NULL` + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rmw_qos_profile_t * +rcl_subscription_get_actual_qos(const rcl_subscription_t * subscription); + +/// Check if subscription instance can loan messages. +/** + * Depending on the middleware and the message type, this will return true if the middleware + * can allocate a ROS message instance. + * + * \param[in] subscription The subscription instance to check for the ability to loan messages + * \return `true` if the subscription instance can loan messages, `false` otherwise. + */ +RCL_PUBLIC +bool +rcl_subscription_can_loan_messages(const rcl_subscription_t * subscription); + +/// Set the on new message callback function for the subscription. +/** + * This API sets the callback function to be called whenever the + * subscription is notified about a new message. + * + * \sa rmw_subscription_set_on_new_message_callback for details about this + * function. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Maybe [1] + * Lock-Free | Maybe [1] + * [1] rmw implementation defined + * + * \param[in] subscription The subscription on which to set the callback + * \param[in] callback The callback to be called when new messages arrive, may be NULL + * \param[in] user_data Given to the callback when called later, may be NULL + * \return `RCL_RET_OK` if successful, or + * \return `RCL_RET_INVALID_ARGUMENT` if `subscription` is NULL, or + * \return `RCL_RET_UNSUPPORTED` if the API is not implemented in the dds implementation + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_subscription_set_on_new_message_callback( + const rcl_subscription_t * subscription, + rcl_event_callback_t callback, + const void * user_data); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__SUBSCRIPTION_H_ diff --git a/include/rcl/time.h b/include/rcl/time.h new file mode 100644 index 0000000..606a79f --- /dev/null +++ b/include/rcl/time.h @@ -0,0 +1,708 @@ +// Copyright 2015 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. + +/// @file + +#ifndef RCL__TIME_H_ +#define RCL__TIME_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/allocator.h" +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" +#include "rcutils/time.h" + +/// Convenience macro to convert seconds to nanoseconds. +#define RCL_S_TO_NS RCUTILS_S_TO_NS +/// Convenience macro to convert milliseconds to nanoseconds. +#define RCL_MS_TO_NS RCUTILS_MS_TO_NS +/// Convenience macro to convert microseconds to nanoseconds. +#define RCL_US_TO_NS RCUTILS_US_TO_NS + +/// Convenience macro to convert nanoseconds to seconds. +#define RCL_NS_TO_S RCUTILS_NS_TO_S +/// Convenience macro to convert nanoseconds to milliseconds. +#define RCL_NS_TO_MS RCUTILS_NS_TO_MS +/// Convenience macro to convert nanoseconds to microseconds. +#define RCL_NS_TO_US RCUTILS_NS_TO_US + +/// A single point in time, measured in nanoseconds since the Unix epoch. +typedef rcutils_time_point_value_t rcl_time_point_value_t; +/// A duration of time, measured in nanoseconds. +typedef rcutils_duration_value_t rcl_duration_value_t; + +/// Time source type, used to indicate the source of a time measurement. +/** + * RCL_ROS_TIME will report the latest value reported by a ROS time source, or + * if a ROS time source is not active it reports the same as RCL_SYSTEM_TIME. + * For more information about ROS time sources, refer to the design document: + * http://design.ros2.org/articles/clock_and_time.html + * + * RCL_SYSTEM_TIME reports the same value as the system clock. + * + * RCL_STEADY_TIME reports a value from a monotonically increasing clock. + */ +typedef enum rcl_clock_type_e +{ + /// Clock uninitialized + RCL_CLOCK_UNINITIALIZED = 0, + /// Use ROS time + RCL_ROS_TIME, + /// Use system time + RCL_SYSTEM_TIME, + /// Use a steady clock time + RCL_STEADY_TIME +} rcl_clock_type_t; + +/// A duration of time, measured in nanoseconds and its source. +typedef struct rcl_duration_s +{ + /// Duration in nanoseconds and its source. + rcl_duration_value_t nanoseconds; +} rcl_duration_t; + +/// Enumeration to describe the type of time jump. +typedef enum rcl_clock_change_e +{ + /// The source before and after the jump is ROS_TIME. + RCL_ROS_TIME_NO_CHANGE = 1, + /// The source switched to ROS_TIME from SYSTEM_TIME. + RCL_ROS_TIME_ACTIVATED = 2, + /// The source switched to SYSTEM_TIME from ROS_TIME. + RCL_ROS_TIME_DEACTIVATED = 3, + /// The source before and after the jump is SYSTEM_TIME. + RCL_SYSTEM_TIME_NO_CHANGE = 4 +} rcl_clock_change_t; + +/// Struct to describe a jump in time. +typedef struct rcl_time_jump_s +{ + /// Indicate whether or not the source of time changed. + rcl_clock_change_t clock_change; + /// The new time minus the last time before the jump. + rcl_duration_t delta; +} rcl_time_jump_t; + +/// Signature of a time jump callback. +/// \param[in] time_jump A description of the jump in time. +/// \param[in] before_jump Every jump callback is called twice: once before the clock changes and +/// once after. This is true the first call and false the second. +/// \param[in] user_data A pointer given at callback registration which is passed to the callback. +typedef void (* rcl_jump_callback_t)( + const rcl_time_jump_t * time_jump, + bool before_jump, + void * user_data); + +/// Describe the prerequisites for calling a time jump callback. +typedef struct rcl_jump_threshold_s +{ + /// True to call callback when the clock type changes. + bool on_clock_change; + /// A positive duration indicating the minimum jump forwards to be considered exceeded, or zero + /// to disable. + rcl_duration_t min_forward; + /// A negative duration indicating the minimum jump backwards to be considered exceeded, or zero + /// to disable. + rcl_duration_t min_backward; +} rcl_jump_threshold_t; + +/// Struct to describe an added callback. +typedef struct rcl_jump_callback_info_s +{ + /// Callback to fucntion. + rcl_jump_callback_t callback; + /// Threshold to decide when to call the callback. + rcl_jump_threshold_t threshold; + /// Pointer passed to the callback. + void * user_data; +} rcl_jump_callback_info_t; + +/// Encapsulation of a time source. +typedef struct rcl_clock_s +{ + /// Clock type + rcl_clock_type_t type; + /// An array of added jump callbacks. + rcl_jump_callback_info_t * jump_callbacks; + /// Number of callbacks in jump_callbacks. + size_t num_jump_callbacks; + /// Pointer to get_now function + rcl_ret_t (* get_now)(void * data, rcl_time_point_value_t * now); + // void (*set_now) (rcl_time_point_value_t); + /// Clock storage + void * data; + /// Custom allocator used for internal allocations. + rcl_allocator_t allocator; +} rcl_clock_t; + +/// A single point in time, measured in nanoseconds, the reference point is based on the source. +typedef struct rcl_time_point_s +{ + /// Nanoseconds of the point in time + rcl_time_point_value_t nanoseconds; + /// Clock type of the point in time + rcl_clock_type_t clock_type; +} rcl_time_point_t; + +// typedef struct rcl_rate_t +// { +// rcl_time_point_value_t trigger_time; +// int64_t period; +// rcl_clock_type_t clock;; +// } rcl_rate_t; +// TODO(tfoote) integrate rate and timer implementations + +/// Check if the clock has valid values. +/** + * This function returns true if the time source appears to be valid. + * It will check that the type is not uninitialized, and that pointers + * are not invalid. + * Note that if data is uninitialized it may give a false positive. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] clock the handle to the clock which is being queried + * \return true if the source is believed to be valid, otherwise return false. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +bool +rcl_clock_valid(rcl_clock_t * clock); + +/// Initialize a clock based on the passed type. +/** + * This will allocate all necessary internal structures, and initialize variables. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes [1] + * Thread-Safe | No [2] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] If `clock_type` is #RCL_ROS_TIME + * [2] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * Thread-safety is also affected by that of the `allocator` object. + * + * \param[in] clock_type the type identifying the time source to provide + * \param[in] clock the handle to the clock which is being initialized + * \param[in] allocator The allocator to use for allocations + * \return #RCL_RET_OK if the time source was successfully initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_clock_init( + rcl_clock_type_t clock_type, rcl_clock_t * clock, + rcl_allocator_t * allocator); + +/// Finalize a clock. +/** + * This will deallocate all necessary internal structures, and clean up any variables. + * It can be combined with any of the init functions. + * + * Passing a clock with type #RCL_CLOCK_UNINITIALIZED will result in + * #RCL_RET_INVALID_ARGUMENT being returned. + * + * This function is not thread-safe with any other function operating on the same + * clock object. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * Thread-safety is also affected by that of the `allocator` object associated with the + * `clock` object. + * + * \param[in] clock the handle to the clock which is being finalized + * \return #RCL_RET_OK if the time source was successfully finalized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_clock_fini( + rcl_clock_t * clock); + +/// Initialize a clock as a #RCL_ROS_TIME time source. +/** + * This will allocate all necessary internal structures, and initialize variables. + * It is specifically setting up a #RCL_ROS_TIME time source. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * + * [2] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * Thread-safety is also affected by that of the `allocator` object. + * + * \param[in] clock the handle to the clock which is being initialized + * \param[in] allocator The allocator to use for allocations + * \return #RCL_RET_OK if the time source was successfully initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_ros_clock_init( + rcl_clock_t * clock, + rcl_allocator_t * allocator); + +/// Finalize a clock as a #RCL_ROS_TIME time source. +/** + * This will deallocate all necessary internal structures, and clean up any variables. + * It is specifically setting up a #RCL_ROS_TIME time source. It is expected + * to be paired with the init fuction. + * + * This function is not thread-safe with any other function operating on the same + * clock object. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * Thread-safety is also affected by that of the `allocator` object associated with the + * `clock` object. + * + * \param[in] clock the handle to the clock which is being initialized + * \return #RCL_RET_OK if the time source was successfully finalized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_ros_clock_fini( + rcl_clock_t * clock); + +/// Initialize a clock as a #RCL_STEADY_TIME time source. +/** + * This will allocate all necessary internal structures, and initialize variables. + * It is specifically setting up a #RCL_STEADY_TIME time source. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * Thread-safety is also affected by that of the `allocator` object. + * + * \param[in] clock the handle to the clock which is being initialized + * \param[in] allocator The allocator to use for allocations + * \return #RCL_RET_OK if the time source was successfully initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_steady_clock_init( + rcl_clock_t * clock, + rcl_allocator_t * allocator); + +/// Finalize a clock as a #RCL_STEADY_TIME time source. +/** + * Finalize the clock as a #RCL_STEADY_TIME time source. + * + * This will deallocate all necessary internal structures, and clean up any variables. + * It is specifically setting up a steady time source. It is expected to be + * paired with the init fuction. + * + * This function is not thread-safe with any other function operating on the same + * clock object. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * Thread-safety is also affected by that of the `allocator` object associated with the + * `clock` object. + * + * \param[in] clock the handle to the clock which is being initialized + * \return #RCL_RET_OK if the time source was successfully finalized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_steady_clock_fini( + rcl_clock_t * clock); + +/// Initialize a clock as a #RCL_SYSTEM_TIME time source. +/** + * Initialize the clock as a #RCL_SYSTEM_TIME time source. + * + * This will allocate all necessary internal structures, and initialize variables. + * It is specifically setting up a system time source. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * Thread-safety is also affected by that of the `allocator` object associated with the + * `clock` object. + * + * \param[in] clock the handle to the clock which is being initialized + * \param[in] allocator The allocator to use for allocations + * \return #RCL_RET_OK if the time source was successfully initialized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_system_clock_init( + rcl_clock_t * clock, + rcl_allocator_t * allocator); + +/// Finalize a clock as a #RCL_SYSTEM_TIME time source. +/** + * Finalize the clock as a #RCL_SYSTEM_TIME time source. + * + * This will deallocate all necessary internal structures, and clean up any variables. + * It is specifically setting up a system time source. It is expected to be paired with + * the init fuction. + * + * This function is not thread-safe with any function operating on the same clock object. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * Thread-safety is also affected by that of the `allocator` object associated with the + * `clock` object. + * + * \param[in] clock the handle to the clock which is being initialized. + * \return #RCL_RET_OK if the time source was successfully finalized, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_system_clock_fini( + rcl_clock_t * clock); + +/// Compute the difference between two time points +/** + * This function takes two time points and computes the duration between them. + * The two time points must be using the same time abstraction, and the + * resultant duration will also be of the same abstraction. + * + * The value will be computed as duration = finish - start. If start is after + * finish the duration will be negative. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] start The time point for the start of the duration. + * \param[in] finish The time point for the end of the duration. + * \param[out] delta The duration between the start and finish. + * \return #RCL_RET_OK if the difference was computed successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_difference_times( + const rcl_time_point_t * start, const rcl_time_point_t * finish, rcl_duration_t * delta); + +/// Fill the time point value with the current value of the associated clock. +/** + * This function will populate the data of the time_point_value object with the + * current value from it's associated time abstraction. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes [1] + * Lock-Free | Yes + * + * [1] If `clock` is of #RCL_ROS_TIME type. + * + * \param[in] clock The time source from which to set the value. + * \param[out] time_point_value The time_point value to populate. + * \return #RCL_RET_OK if the last call time was retrieved successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_value_t * time_point_value); + + +/// Enable the ROS time abstraction override. +/** + * This method will enable the ROS time abstraction override values, + * such that the time source will report the set value instead of falling + * back to system time. + * + * This function is not thread-safe with rcl_clock_add_jump_callback(), + * nor rcl_clock_remove_jump_callback() functions when used on the same + * clock object. + * + *
+ * Attribute | Adherence [1] + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No [2] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] Only applies to the function itself, as jump callbacks may not abide to it. + * [2] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * + * \param[in] clock The clock to enable. + * \return #RCL_RET_OK if the time source was enabled successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_enable_ros_time_override(rcl_clock_t * clock); + +/// Disable the ROS time abstraction override. +/** + * This method will disable the #RCL_ROS_TIME time abstraction override values, + * such that the time source will report the system time even if a custom + * value has been set. + * + * This function is not thread-safe with rcl_clock_add_jump_callback(), + * nor rcl_clock_remove_jump_callback() functions when used on the same + * clock object. + * + *
+ * Attribute | Adherence [1] + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No [2] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] Only applies to the function itself, as jump callbacks may not abide to it. + * [2] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * + * \param[in] clock The clock to disable. + * \return #RCL_RET_OK if the time source was disabled successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_disable_ros_time_override(rcl_clock_t * clock); + + +/// Check if the #RCL_ROS_TIME time source has the override enabled. +/** + * This will populate the is_enabled object to indicate if the + * time overide is enabled. If it is enabled, the set value will be returned. + * Otherwise this time source will return the equivalent to system time abstraction. + * + * This function is not thread-safe with rcl_enable_ros_time_override() nor + * rcl_disable_ros_time_override() functions when used on the same clock object. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * + * \param[in] clock The clock to query. + * \param[out] is_enabled Whether the override is enabled.. + * \return #RCL_RET_OK if the time source was queried successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_is_enabled_ros_time_override( + rcl_clock_t * clock, bool * is_enabled); + +/// Set the current time for this #RCL_ROS_TIME time source. +/** + * This function will update the internal storage for the #RCL_ROS_TIME + * time source. + * If queried and override enabled the time source will return this value, + * otherwise it will return the system time. + * + * This function is not thread-safe with rcl_clock_add_jump_callback(), + * nor rcl_clock_remove_jump_callback() functions when used on the same + * clock object. + * + *
+ * Attribute | Adherence [1] + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No [2] + * Uses Atomics | Yes + * Lock-Free | Yes + * + * [1] Only applies to the function itself, as jump callbacks may not abide to it. + * [2] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * + * \param[in] clock The clock to update. + * \param[in] time_value The new current time. + * \return #RCL_RET_OK if the time source was set successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_set_ros_time_override( + rcl_clock_t * clock, rcl_time_point_value_t time_value); + +/// Add a callback to be called when a time jump exceeds a threshold. +/** + * The callback is called twice when the threshold is exceeded: once before the clock is + * updated, and once after. + * The user_data pointer is passed to the callback as the last argument. + * A callback and user_data pair must be unique among the callbacks added to a clock. + * + * This function is not thread-safe with rcl_clock_remove_jump_callback(), + * rcl_enable_ros_time_override(), rcl_disable_ros_time_override() nor + * rcl_set_ros_time_override() functions when used on the same clock object. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * Thread-safety is also affected by that of the `allocator` object associated with the + * `clock` object. + * + * \param[in] clock A clock to add a jump callback to. + * \param[in] threshold Criteria indicating when to call the callback. + * \param[in] callback A callback to call. + * \param[in] user_data A pointer to be passed to the callback. + * \return #RCL_RET_OK if the callback was added successfully, or + * \return #RCL_RET_BAD_ALLOC if a memory allocation failed, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_clock_add_jump_callback( + rcl_clock_t * clock, rcl_jump_threshold_t threshold, rcl_jump_callback_t callback, + void * user_data); + +/// Remove a previously added time jump callback. +/** + * This function is not thread-safe with rcl_clock_add_jump_callback() + * rcl_enable_ros_time_override(), rcl_disable_ros_time_override() nor + * rcl_set_ros_time_override() functions when used on the same clock object. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No [1] + * Uses Atomics | No + * Lock-Free | Yes + * + * [1] Function is reentrant, but concurrent calls on the same `clock` object are not safe. + * Thread-safety is also affected by that of the `allocator` object associated with the + * `clock` object. + * + * \param[in] clock The clock to remove a jump callback from. + * \param[in] callback The callback to call. + * \param[in] user_data A pointer to be passed to the callback. + * \return #RCL_RET_OK if the callback was added successfully, or + * \return #RCL_RET_BAD_ALLOC if a memory allocation failed, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR the callback was not found or an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_clock_remove_jump_callback( + rcl_clock_t * clock, rcl_jump_callback_t callback, void * user_data); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__TIME_H_ diff --git a/include/rcl/timer.h b/include/rcl/timer.h new file mode 100644 index 0000000..ec08acf --- /dev/null +++ b/include/rcl/timer.h @@ -0,0 +1,597 @@ +// Copyright 2015 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. + +/// @file + +#ifndef RCL__TIMER_H_ +#define RCL__TIMER_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +#include "rcl/allocator.h" +#include "rcl/context.h" +#include "rcl/guard_condition.h" +#include "rcl/macros.h" +#include "rcl/time.h" +#include "rcl/types.h" +#include "rmw/rmw.h" + +typedef struct rcl_timer_impl_s rcl_timer_impl_t; + +/// Structure which encapsulates a ROS Timer. +typedef struct rcl_timer_s +{ + /// Private implementation pointer. + rcl_timer_impl_t * impl; +} rcl_timer_t; + +/// User callback signature for timers. +/** + * The first argument the callback gets is a pointer to the timer. + * This can be used to cancel the timer, query the time until the next + * timer callback, exchange the callback with a different one, etc. + * + * The only caveat is that the function rcl_timer_get_time_since_last_call() + * will return the time since just before this callback was called, not the + * previous call. + * Therefore the second argument given is the time since the previous callback + * was called, because that information is no longer accessible via the timer. + * The time since the last callback call is given in nanoseconds. + */ +typedef void (* rcl_timer_callback_t)(rcl_timer_t *, int64_t); + +/// Return a zero initialized timer. +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_timer_t +rcl_get_zero_initialized_timer(void); + +/// Initialize a timer. +/** + * A timer consists of a clock, a callback function and a period. + * A timer can be added to a wait set and waited on, such that the wait set + * will wake up when a timer is ready to be executed. + * + * A timer simply holds state and does not automatically call callbacks. + * It does not create any threads, register interrupts, or consume signals. + * For blocking behavior it can be used in conjunction with a wait set and + * rcl_wait(). + * When rcl_timer_is_ready() returns true, the timer must still be called + * explicitly using rcl_timer_call(). + * + * The timer handle must be a pointer to an allocated and zero initialized + * rcl_timer_t struct. + * Calling this function on an already initialized timer will fail. + * Calling this function on a timer struct which has been allocated but not + * zero initialized is undefined behavior. + * + * The clock handle must be a pointer to an initialized rcl_clock_t struct. + * The life time of the clock must exceed the life time of the timer. + * + * The period is a non-negative duration (rather an absolute time in the + * future). + * If the period is `0` then it will always be ready. + * + * The callback is an optional argument. + * Valid inputs are either a pointer to the function callback, or `NULL` to + * indicate that no callback will be stored in rcl. + * If the callback is `NULL`, the caller client library is responsible for + * firing the timer callback. + * Else, it must be a function which returns void and takes two arguments, + * the first being a pointer to the associated timer, and the second a int64_t + * which is the time since the previous call, or since the timer was created + * if it is the first call to the callback. + * + * Expected usage: + * + * ```c + * #include + * + * void my_timer_callback(rcl_timer_t * timer, int64_t last_call_time) + * { + * // Do timer work... + * // Optionally reconfigure, cancel, or reset the timer... + * } + * + * rcl_context_t * context; // initialized previously by rcl_init()... + * rcl_clock_t clock; + * rcl_allocator_t allocator = rcl_get_default_allocator(); + * rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + * // ... error handling + * + * rcl_timer_t timer = rcl_get_zero_initialized_timer(); + * ret = rcl_timer_init( + * &timer, &clock, context, RCL_MS_TO_NS(100), my_timer_callback, allocator); + * // ... error handling, use the timer with a wait set, or poll it manually, then cleanup + * ret = rcl_timer_fini(&timer); + * // ... error handling + * ``` + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Yes + * Lock-Free | Yes [1][2][3] + * [1] if `atomic_is_lock_free()` returns true for `atomic_uintptr_t` + * + * [2] if `atomic_is_lock_free()` returns true for `atomic_uint_least64_t` + * + * [3] if `atomic_is_lock_free()` returns true for `atomic_bool` + * + * \param[inout] timer the timer handle to be initialized + * \param[in] clock the clock providing the current time + * \param[in] context the context that this timer is to be associated with + * \param[in] period the duration between calls to the callback in nanoseconds + * \param[in] callback the user defined function to be called every period + * \param[in] allocator the allocator to use for allocations + * \return #RCL_RET_OK if the timer was initialized successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ALREADY_INIT if the timer was already initialized, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_init( + rcl_timer_t * timer, + rcl_clock_t * clock, + rcl_context_t * context, + int64_t period, + const rcl_timer_callback_t callback, + rcl_allocator_t allocator); + +/// Finalize a timer. +/** + * This function will deallocate any memory and make the timer invalid. + * + * A timer that is already invalid (zero initialized) or `NULL` will not fail. + * + * This function is not thread-safe with any rcl_timer_* functions used on the + * same timer object. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | Yes + * Lock-Free | Yes [1][2][3] + * [1] if `atomic_is_lock_free()` returns true for `atomic_uintptr_t` + * + * [2] if `atomic_is_lock_free()` returns true for `atomic_uint_least64_t` + * + * [3] if `atomic_is_lock_free()` returns true for `atomic_bool` + * + * \param[inout] timer the handle to the timer to be finalized. + * \return #RCL_RET_OK if the timer was finalized successfully, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_fini(rcl_timer_t * timer); + +/// Call the timer's callback and set the last call time. +/** + * This function will call the callback and change the last call time even if + * the timer's period has not yet elapsed. + * It is up to the calling code to make sure the period has elapsed by first + * calling rcl_timer_is_ready(). + * If the callback pointer is `NULL` (either set in init or exchanged after + * initialized), no callback is fired. + * However, this function should still be called by the client library to + * update the state of the timer. + * The order of operations in this command are as follows: + * + * - Ensure the timer has not been canceled. + * - Get the current time into a temporary rcl_steady_time_point_t. + * - Exchange the current time with the last call time of the timer. + * - Call the callback, passing this timer and the time since the last call. + * - Return after the callback has completed. + * + * During the callback the timer can be canceled or have its period and/or + * callback modified. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes [1] + * Uses Atomics | Yes + * Lock-Free | Yes [2] + * [1] user callback might not be thread-safe + * + * [2] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t` + * + * \param[inout] timer the handle to the timer to call + * \return #RCL_RET_OK if the timer was called successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMER_INVALID if the timer->impl is invalid, or + * \return #RCL_RET_TIMER_CANCELED if the timer has been canceled, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_call(rcl_timer_t * timer); + +/// Retrieve the clock of the timer. +/** + * This function retrieves the clock pointer and copies it into the given variable. + * + * The clock argument must be a pointer to an already allocated rcl_clock_t *. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] timer the handle to the timer which is being queried + * \param[out] clock the rcl_clock_t * in which the clock is stored + * \return #RCL_RET_OK if the clock was retrieved successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMER_INVALID if the timer is invalid. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_clock(rcl_timer_t * timer, rcl_clock_t ** clock); + +/// Calculates whether or not the timer should be called. +/** + * The result is true if the time until next call is less than, or equal to, 0 + * and the timer has not been canceled. + * Otherwise the result is false, indicating the timer should not be called. + * + * The is_ready argument must point to an allocated bool object, as the result + * is copied into it. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t` + * + * \param[in] timer the handle to the timer which is being checked + * \param[out] is_ready the bool used to store the result of the calculation + * \return #RCL_RET_OK if the last call time was retrieved successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMER_INVALID if the timer->impl is invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready); + +/// Calculate and retrieve the time until the next call in nanoseconds. +/** + * This function calculates the time until the next call by adding the timer's + * period to the last call time and subtracting that sum from the current time. + * The calculated time until the next call can be positive, indicating that it + * is not ready to be called as the period has not elapsed since the last call. + * The calculated time until the next call can also be 0 or negative, + * indicating that the period has elapsed since the last call and the timer + * should be called. + * A negative value indicates the timer call is overdue by that amount. + * + * The `time_until_next_call` argument must point to an allocated int64_t, as + * the time until is copied into that instance. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t` + * + * \param[in] timer the handle to the timer that is being queried + * \param[out] time_until_next_call the output variable for the result + * \return #RCL_RET_OK if the timer until next call was successfully calculated, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMER_INVALID if the timer->impl is invalid, or + * \return #RCL_RET_TIMER_CANCELED if the timer is canceled, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call); + +/// Retrieve the time since the previous call to rcl_timer_call() occurred. +/** + * This function calculates the time since the last call and copies it into + * the given int64_t variable. + * + * Calling this function within a callback will not return the time since the + * previous call but instead the time since the current callback was called. + * + * The time_since_last_call argument must be a pointer to an already allocated + * int64_t. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t` + * + * \param[in] timer the handle to the timer which is being queried + * \param[out] time_since_last_call the struct in which the time is stored + * \return #RCL_RET_OK if the last call time was retrieved successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMER_INVALID if the timer->impl is invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_get_time_since_last_call(const rcl_timer_t * timer, int64_t * time_since_last_call); + +/// Retrieve the period of the timer. +/** + * This function retrieves the period and copies it into the given variable. + * + * The period argument must be a pointer to an already allocated int64_t. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t` + * + * \param[in] timer the handle to the timer which is being queried + * \param[out] period the int64_t in which the period is stored + * \return #RCL_RET_OK if the period was retrieved successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMER_INVALID if the timer->impl is invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_get_period(const rcl_timer_t * timer, int64_t * period); + +/// Exchange the period of the timer and return the previous period. +/** + * This function exchanges the period in the timer and copies the old one into + * the given variable. + * + * Exchanging (changing) the period will not affect already waiting wait sets. + * + * The old_period argument must be a pointer to an already allocated int64_t. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t` + * + * \param[in] timer the handle to the timer which is being modified + * \param[out] new_period the int64_t to exchange into the timer + * \param[out] old_period the int64_t in which the previous period is stored + * \return #RCL_RET_OK if the period was retrieved successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMER_INVALID if the timer->impl is invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_exchange_period(const rcl_timer_t * timer, int64_t new_period, int64_t * old_period); + +/// Return the current timer callback. +/** + * This function can fail, and therefore return `NULL`, if: + * - timer is `NULL` + * - timer has not been initialized (the implementation is invalid) + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t` + * + * \param[in] timer handle to the timer from the callback should be returned + * \return function pointer to the callback, or `NULL` if an error occurred + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_timer_callback_t +rcl_timer_get_callback(const rcl_timer_t * timer); + +/// Exchange the current timer callback and return the current callback. +/** + * This function can fail, and therefore return `NULL`, if: + * - timer is `NULL` + * - timer has not been initialized (the implementation is invalid) + * + * This function can set callback to `NULL`, in which case the callback is + * ignored when rcl_timer_call is called. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t` + * + * \param[inout] timer handle to the timer from the callback should be exchanged + * \param[in] new_callback the callback to be exchanged into the timer + * \return function pointer to the old callback, or `NULL` if an error occurred + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_timer_callback_t +rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback); + +/// Cancel a timer. +/** + * When a timer is canceled, rcl_timer_is_ready() will return false for that + * timer, and rcl_timer_call() will fail with RCL_RET_TIMER_CANCELED. + * + * A canceled timer can be reset with rcl_timer_reset(), and then used again. + * Calling this function on an already canceled timer will succeed. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t` + * + * \param[inout] timer the timer to be canceled + * \return #RCL_RET_OK if the timer was canceled successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMER_INVALID if the timer is invalid. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_cancel(rcl_timer_t * timer); + +/// Retrieve the canceled state of a timer. +/** + * If the timer is canceled true will be stored in the is_canceled argument. + * Otherwise false will be stored in the is_canceled argument. + * + * The is_canceled argument must point to an allocated bool, as the result is + * copied into this variable. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_bool` + * + * \param[in] timer the timer to be queried + * \param[out] is_canceled storage for the is canceled bool + * \return #RCL_RET_OK if the last call time was retrieved successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMER_INVALID if the timer->impl is invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled); + +/// Reset a timer. +/** + * This function can be called on a timer, canceled or not. + * For all timers it will reset the last call time to now. + * For canceled timers it will additionally make the timer not canceled. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | Yes + * Lock-Free | Yes [1] + * [1] if `atomic_is_lock_free()` returns true for `atomic_int_least64_t` + * + * \param[inout] timer the timer to be reset + * \return #RCL_RET_OK if the timer was reset successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_TIMER_INVALID if the timer is invalid, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_reset(rcl_timer_t * timer); + +/// Return the allocator for the timer. +/** + * This function can fail, and therefore return `NULL`, if: + * - timer is `NULL` + * - timer has not been initialized (the implementation is invalid) + * + * The returned pointer is only valid as long as the timer object is valid. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | Yes + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] timer handle to the timer object + * \return pointer to the allocator, or `NULL` if an error occurred + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const rcl_allocator_t * +rcl_timer_get_allocator(const rcl_timer_t * timer); + +/// Retrieve a guard condition used by the timer to wake the waitset when using ROSTime. +/** + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] timer the timer to be queried + * \return `NULL` if the timer is invalid or does not have a guard condition, or + * \return a guard condition pointer. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_guard_condition_t * +rcl_timer_get_guard_condition(const rcl_timer_t * timer); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__TIMER_H_ diff --git a/include/rcl/types.h b/include/rcl/types.h new file mode 100644 index 0000000..15ef8b5 --- /dev/null +++ b/include/rcl/types.h @@ -0,0 +1,129 @@ +// Copyright 2014 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. + +/// @file + +#ifndef RCL__TYPES_H_ +#define RCL__TYPES_H_ + +#include + +/// The type that holds an rcl return code. +typedef rmw_ret_t rcl_ret_t; + +/// Success return code. +#define RCL_RET_OK RMW_RET_OK +/// Unspecified error return code. +#define RCL_RET_ERROR RMW_RET_ERROR +/// Timeout occurred return code. +#define RCL_RET_TIMEOUT RMW_RET_TIMEOUT +/// Failed to allocate memory return code. +#define RCL_RET_BAD_ALLOC RMW_RET_BAD_ALLOC +/// Invalid argument return code. +#define RCL_RET_INVALID_ARGUMENT RMW_RET_INVALID_ARGUMENT +/// Unsupported return code. +#define RCL_RET_UNSUPPORTED RMW_RET_UNSUPPORTED + +// rcl specific ret codes start at 100 +/// rcl_init() already called return code. +#define RCL_RET_ALREADY_INIT 100 +/// rcl_init() not yet called return code. +#define RCL_RET_NOT_INIT 101 +/// Mismatched rmw identifier return code. +#define RCL_RET_MISMATCHED_RMW_ID 102 +/// Topic name does not pass validation. +#define RCL_RET_TOPIC_NAME_INVALID 103 +/// Service name (same as topic name) does not pass validation. +#define RCL_RET_SERVICE_NAME_INVALID 104 +/// Topic name substitution is unknown. +#define RCL_RET_UNKNOWN_SUBSTITUTION 105 +/// rcl_shutdown() already called return code. +#define RCL_RET_ALREADY_SHUTDOWN 106 + +// rcl node specific ret codes in 2XX +/// Invalid rcl_node_t given return code. +#define RCL_RET_NODE_INVALID 200 +/// Invalid node name return code. +#define RCL_RET_NODE_INVALID_NAME 201 +/// Invalid node namespace return code. +#define RCL_RET_NODE_INVALID_NAMESPACE 202 +/// Failed to find node name +#define RCL_RET_NODE_NAME_NON_EXISTENT 203 + +// rcl publisher specific ret codes in 3XX +/// Invalid rcl_publisher_t given return code. +#define RCL_RET_PUBLISHER_INVALID 300 + +// rcl subscription specific ret codes in 4XX +/// Invalid rcl_subscription_t given return code. +#define RCL_RET_SUBSCRIPTION_INVALID 400 +/// Failed to take a message from the subscription return code. +#define RCL_RET_SUBSCRIPTION_TAKE_FAILED 401 + +// rcl service client specific ret codes in 5XX +/// Invalid rcl_client_t given return code. +#define RCL_RET_CLIENT_INVALID 500 +/// Failed to take a response from the client return code. +#define RCL_RET_CLIENT_TAKE_FAILED 501 + +// rcl service server specific ret codes in 6XX +/// Invalid rcl_service_t given return code. +#define RCL_RET_SERVICE_INVALID 600 +/// Failed to take a request from the service return code. +#define RCL_RET_SERVICE_TAKE_FAILED 601 + +// rcl guard condition specific ret codes in 7XX + +// rcl timer specific ret codes in 8XX +/// Invalid rcl_timer_t given return code. +#define RCL_RET_TIMER_INVALID 800 +/// Given timer was canceled return code. +#define RCL_RET_TIMER_CANCELED 801 + +// rcl wait and wait set specific ret codes in 9XX +/// Invalid rcl_wait_set_t given return code. +#define RCL_RET_WAIT_SET_INVALID 900 +/// Given rcl_wait_set_t is empty return code. +#define RCL_RET_WAIT_SET_EMPTY 901 +/// Given rcl_wait_set_t is full return code. +#define RCL_RET_WAIT_SET_FULL 902 + +// rcl argument parsing specific ret codes in 1XXX +/// Argument is not a valid remap rule +#define RCL_RET_INVALID_REMAP_RULE 1001 +/// Expected one type of lexeme but got another +#define RCL_RET_WRONG_LEXEME 1002 +/// Found invalid ros argument while parsing +#define RCL_RET_INVALID_ROS_ARGS 1003 +/// Argument is not a valid parameter rule +#define RCL_RET_INVALID_PARAM_RULE 1010 +/// Argument is not a valid log level rule +#define RCL_RET_INVALID_LOG_LEVEL_RULE 1020 + +// rcl event specific ret codes in 20XX +/// Invalid rcl_event_t given return code. +#define RCL_RET_EVENT_INVALID 2000 +/// Failed to take an event from the event handle +#define RCL_RET_EVENT_TAKE_FAILED 2001 + +/// rcl_lifecycle state register ret codes in 30XX +/// rcl_lifecycle state registered +#define RCL_RET_LIFECYCLE_STATE_REGISTERED 3000 +/// rcl_lifecycle state not registered +#define RCL_RET_LIFECYCLE_STATE_NOT_REGISTERED 3001 + +/// typedef for rmw_serialized_message_t; +typedef rmw_serialized_message_t rcl_serialized_message_t; + +#endif // RCL__TYPES_H_ diff --git a/include/rcl/validate_enclave_name.h b/include/rcl/validate_enclave_name.h new file mode 100644 index 0000000..d6c44de --- /dev/null +++ b/include/rcl/validate_enclave_name.h @@ -0,0 +1,120 @@ +// 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. + +/// @file + +#ifndef RCL__VALIDATE_ENCLAVE_NAME_H_ +#define RCL__VALIDATE_ENCLAVE_NAME_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +/// The enclave name is valid. +#define RCL_ENCLAVE_NAME_VALID RMW_NAMESPACE_VALID + +/// The enclave name is invalid because it is an empty string. +#define RCL_ENCLAVE_NAME_INVALID_IS_EMPTY_STRING RMW_NAMESPACE_INVALID_IS_EMPTY_STRING + +/// The enclave name is invalid because it is not absolute. +#define RCL_ENCLAVE_NAME_INVALID_NOT_ABSOLUTE RMW_NAMESPACE_INVALID_NOT_ABSOLUTE + +/// The enclave name is invalid because it ends with a forward slash. +#define RCL_ENCLAVE_NAME_INVALID_ENDS_WITH_FORWARD_SLASH \ + RMW_NAMESPACE_INVALID_ENDS_WITH_FORWARD_SLASH + +/// The enclave name is invalid because it has characters that are not allowed. +#define RCL_ENCLAVE_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS \ + RMW_NAMESPACE_INVALID_CONTAINS_UNALLOWED_CHARACTERS + +/// The enclave name is invalid because it contains repeated forward slashes. +#define RCL_ENCLAVE_NAME_INVALID_CONTAINS_REPEATED_FORWARD_SLASH \ + RMW_NAMESPACE_INVALID_CONTAINS_REPEATED_FORWARD_SLASH + +/// The enclave name is invalid because one of the tokens starts with a number. +#define RCL_ENCLAVE_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER \ + RMW_NAMESPACE_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER + +/// The enclave name is invalid because the name is too long. +#define RCL_ENCLAVE_NAME_INVALID_TOO_LONG RMW_NAMESPACE_INVALID_TOO_LONG + +/// The maximum length of an enclave name. +#define RCL_ENCLAVE_NAME_MAX_LENGTH RMW_NODE_NAME_MAX_NAME_LENGTH + +/// Determine if a given enclave name is valid. +/** + * The same rules as rmw_validate_namespace() are used. + * The only difference is in the maximum allowed length, which can be up to 255 characters. + * + * \param[in] enclave enclave to be validated + * \param[out] validation_result int in which the result of the check is stored + * \param[out] invalid_index index of the input string where an error occurred + * \return #RCL_RET_OK on successfully running the check, or + * \return #RCL_RET_INVALID_ARGUMENT on invalid parameters, or + * \return #RCL_RET_ERROR when an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_validate_enclave_name( + const char * enclave, + int * validation_result, + size_t * invalid_index); + +/// Deterimine if a given enclave name is valid. +/** + * This is an overload of rcl_validate_enclave_name() with an extra parameter + * for the length of enclave. + * + * \param[in] enclave enclave to be validated + * \param[in] enclave_length The number of characters in enclave + * \param[out] validation_result int in which the result of the check is stored + * \param[out] invalid_index index of the input string where an error occurred + * \return #RCL_RET_OK on successfully running the check, or + * \return #RCL_RET_INVALID_ARGUMENT on invalid parameters, or + * \return #RCL_RET_ERROR when an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_validate_enclave_name_with_size( + const char * enclave, + size_t enclave_length, + int * validation_result, + size_t * invalid_index); + +/// Return a validation result description, or NULL if unknown or RCL_ENCLAVE_NAME_VALID. +/** + * \param[in] validation_result The validation result to get the string for + * \return A string description of the validation result if successful, or + * \return NULL if the validation result is invalid. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_enclave_name_validation_result_string(int validation_result); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__VALIDATE_ENCLAVE_NAME_H_ diff --git a/include/rcl/validate_topic_name.h b/include/rcl/validate_topic_name.h new file mode 100644 index 0000000..76696f2 --- /dev/null +++ b/include/rcl/validate_topic_name.h @@ -0,0 +1,155 @@ +// 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. + +/// @file + +#ifndef RCL__VALIDATE_TOPIC_NAME_H_ +#define RCL__VALIDATE_TOPIC_NAME_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/macros.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +/// The topic name is valid. +#define RCL_TOPIC_NAME_VALID 0 + +/// The topic name is invalid because it is an empty string. +#define RCL_TOPIC_NAME_INVALID_IS_EMPTY_STRING 1 + +/// The topic name is invalid because it ends with a forward slash. +#define RCL_TOPIC_NAME_INVALID_ENDS_WITH_FORWARD_SLASH 2 + +/// The topic name is invalid because it has characters that are not allowed. +#define RCL_TOPIC_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS 3 + +/// The topic name is invalid because one of the tokens starts with a number. +#define RCL_TOPIC_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER 4 + +/// The topic name is invalid because it has an unmatched curly brace. +#define RCL_TOPIC_NAME_INVALID_UNMATCHED_CURLY_BRACE 5 + +/// The topic name is invalid because it has a misplaced tilde in it. +#define RCL_TOPIC_NAME_INVALID_MISPLACED_TILDE 6 + +/// The topic name is invalid because a tilde is not directly followed by a forward slash. +#define RCL_TOPIC_NAME_INVALID_TILDE_NOT_FOLLOWED_BY_FORWARD_SLASH 7 + +/// The topic name is invalid because one of the substitutions has characters that are not allowed. +#define RCL_TOPIC_NAME_INVALID_SUBSTITUTION_CONTAINS_UNALLOWED_CHARACTERS 8 + +/// The topic name is invalid because one of the substitutions starts with a number. +#define RCL_TOPIC_NAME_INVALID_SUBSTITUTION_STARTS_WITH_NUMBER 9 + +/// Validate a given topic name. +/** + * The topic name does not need to be a full qualified name, but it should + * follow some of the rules in this document: + * + * http://design.ros2.org/articles/topic_and_service_names.html + * + * Note that this function expects any URL suffixes as described in the above + * document to have already been removed. + * + * If the input topic is valid, RCL_TOPIC_NAME_VALID will be stored + * into validation_result. + * If the input topic violates any of the rules then one of these values will + * be stored into validation_result: + * + * - RCL_TOPIC_NAME_INVALID_IS_EMPTY_STRING + * - RCL_TOPIC_NAME_INVALID_ENDS_WITH_FORWARD_SLASH + * - RCL_TOPIC_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS + * - RCL_TOPIC_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER + * - RCL_TOPIC_NAME_INVALID_UNMATCHED_CURLY_BRACE + * - RCL_TOPIC_NAME_INVALID_MISPLACED_TILDE + * - RCL_TOPIC_NAME_INVALID_TILDE_NOT_FOLLOWED_BY_FORWARD_SLASH + * - RCL_TOPIC_NAME_INVALID_SUBSTITUTION_CONTAINS_UNALLOWED_CHARACTERS + * - RCL_TOPIC_NAME_INVALID_SUBSTITUTION_STARTS_WITH_NUMBER + * + * Some checks, like the check for illegal repeated forward slashes, are not + * checked in this function because they would need to be checked again after + * expansion anyways. + * The purpose of this subset of checks is to try to catch issues with content + * that will be expanded in some way by rcl_expand_topic_name(), like `~` or + * substitutions inside of `{}`, or with other issues that would obviously + * prevent expansion, like RCL_TOPIC_NAME_INVALID_IS_EMPTY_STRING. + * + * This function does not check that the substitutions are known substitutions, + * only that the contents of the `{}` follow the rules outline in the document + * which was linked to above. + * + * Stricter validation can be done with rmw_validate_full_topic_name() after using + * rcl_expand_topic_name(). + * + * Additionally, if the invalid_index argument is not NULL, then it will be + * assigned the index in the topic_name string where the violation occurs. + * The invalid_index is not set if the validation passes. + * + * \param[in] topic_name the topic name to be validated, must be null terminated + * \param[out] validation_result the reason for validation failure, if any + * \param[out] invalid_index index of violation if the input topic is invalid + * \return #RCL_RET_OK if the topic name was expanded successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_validate_topic_name( + const char * topic_name, + int * validation_result, + size_t * invalid_index); + +/// Validate a given topic name. +/** + * This is an overload with an extra parameter for the length of topic_name. + * \param[in] topic_name the topic name to be validated, must be null terminated + * \param[in] topic_name_length The number of characters in topic_name. + * \param[out] validation_result the reason for validation failure, if any + * \param[out] invalid_index index of violation if the input topic is invalid + * \return #RCL_RET_OK if the topic name was expanded successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + * + * \sa rcl_validate_topic_name() + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_validate_topic_name_with_size( + const char * topic_name, + size_t topic_name_length, + int * validation_result, + size_t * invalid_index); + +/// Return a validation result description, or NULL if unknown or RCL_TOPIC_NAME_VALID. +/** + * \param[in] validation_result The validation result to get the string for + * \return A string description of the validation result if successful, or + * \return NULL if the validation result is invalid. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +const char * +rcl_topic_name_validation_result_string(int validation_result); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__VALIDATE_TOPIC_NAME_H_ diff --git a/include/rcl/visibility_control.h b/include/rcl/visibility_control.h new file mode 100644 index 0000000..9c8c497 --- /dev/null +++ b/include/rcl/visibility_control.h @@ -0,0 +1,58 @@ +// Copyright 2015 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 RCL__VISIBILITY_CONTROL_H_ +#define RCL__VISIBILITY_CONTROL_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// 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 RCL_EXPORT __attribute__ ((dllexport)) + #define RCL_IMPORT __attribute__ ((dllimport)) + #else + #define RCL_EXPORT __declspec(dllexport) + #define RCL_IMPORT __declspec(dllimport) + #endif + #ifdef RCL_BUILDING_DLL + #define RCL_PUBLIC RCL_EXPORT + #else + #define RCL_PUBLIC RCL_IMPORT + #endif + #define RCL_PUBLIC_TYPE RCL_PUBLIC + #define RCL_LOCAL +#else + #define RCL_EXPORT __attribute__ ((visibility("default"))) + #define RCL_IMPORT + #if __GNUC__ >= 4 + #define RCL_PUBLIC __attribute__ ((visibility("default"))) + #define RCL_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define RCL_PUBLIC + #define RCL_LOCAL + #endif + #define RCL_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // RCL__VISIBILITY_CONTROL_H_ diff --git a/include/rcl/wait.h b/include/rcl/wait.h new file mode 100644 index 0000000..60e21a2 --- /dev/null +++ b/include/rcl/wait.h @@ -0,0 +1,502 @@ +// Copyright 2015 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. + +/// @file + +#ifndef RCL__WAIT_H_ +#define RCL__WAIT_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include + +#include "rcl/client.h" +#include "rcl/guard_condition.h" +#include "rcl/macros.h" +#include "rcl/service.h" +#include "rcl/subscription.h" +#include "rcl/timer.h" +#include "rcl/event.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +typedef struct rcl_wait_set_impl_s rcl_wait_set_impl_t; + +/// Container for subscription's, guard condition's, etc to be waited on. +typedef struct rcl_wait_set_s +{ + /// Storage for subscription pointers. + const rcl_subscription_t ** subscriptions; + /// Number of subscriptions + size_t size_of_subscriptions; + /// Storage for guard condition pointers. + const rcl_guard_condition_t ** guard_conditions; + /// Number of guard_conditions + size_t size_of_guard_conditions; + /// Storage for timer pointers. + const rcl_timer_t ** timers; + /// Number of timers + size_t size_of_timers; + /// Storage for client pointers. + const rcl_client_t ** clients; + /// Number of clients + size_t size_of_clients; + /// Storage for service pointers. + const rcl_service_t ** services; + /// Number of services + size_t size_of_services; + /// Storage for event pointers. + const rcl_event_t ** events; + /// Number of events + size_t size_of_events; + /// Implementation specific storage. + rcl_wait_set_impl_t * impl; +} rcl_wait_set_t; + +/// Return a rcl_wait_set_t struct with members set to `NULL`. +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_wait_set_t +rcl_get_zero_initialized_wait_set(void); + +/// Initialize a rcl wait set with space for items to be waited on. +/** + * This function allocates space for the subscriptions and other wait-able + * entities that can be stored in the wait set. + * It also sets the allocator to the given allocator and initializes the pruned + * member to be false. + * + * The wait_set struct should be allocated and initialized to `NULL`. + * If the wait_set is allocated but the memory is uninitialized the behavior is + * undefined. + * Calling this function on a wait set that has already been initialized will + * result in an error. + * A wait set can be reinitialized if rcl_wait_set_fini() was called on it. + * + * To use the default allocator use rcl_get_default_allocator(). + * + * Expected usage: + * + * ```c + * #include + * + * rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + * rcl_ret_t ret = + * rcl_wait_set_init(&wait_set, 42, 42, 42, 42, 42, &context, rcl_get_default_allocator()); + * // ... error handling, then use it, then call the matching fini: + * ret = rcl_wait_set_fini(&wait_set); + * // ... error handling + * ``` + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] wait_set the wait set struct to be initialized + * \param[in] number_of_subscriptions non-zero size of the subscriptions set + * \param[in] number_of_guard_conditions non-zero size of the guard conditions set + * \param[in] number_of_timers non-zero size of the timers set + * \param[in] number_of_clients non-zero size of the clients set + * \param[in] number_of_services non-zero size of the services set + * \param[in] number_of_events non-zero size of the events set + * \param[in] context the context that the wait set should be associated with + * \param[in] allocator the allocator to use when allocating space in the sets + * \return #RCL_RET_OK if the wait set is initialized successfully, or + * \return #RCL_RET_ALREADY_INIT if the wait set is not zero initialized, or + * \return #RCL_RET_NOT_INIT if the given context is invalid, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_WAIT_SET_INVALID if the wait set is not destroyed properly, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_init( + rcl_wait_set_t * wait_set, + size_t number_of_subscriptions, + size_t number_of_guard_conditions, + size_t number_of_timers, + size_t number_of_clients, + size_t number_of_services, + size_t number_of_events, + rcl_context_t * context, + rcl_allocator_t allocator); + +/// Finalize a rcl wait set. +/** + * Deallocates any memory in the wait set that was allocated in + * rcl_wait_set_init() using the allocator given in the initialization. + * + * Calling this function on a zero initialized wait set will do nothing and + * return RCL_RET_OK. + * Calling this function on uninitialized memory results in undefined behavior. + * After calling this function the wait set will once again be zero initialized + * and so calling this function or rcl_wait_set_init() immediately after will + * succeed. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] wait_set the wait set struct to be finalized. + * \return #RCL_RET_OK if the finalization was successful, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_WAIT_SET_INVALID if the wait set is not destroyed properly, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_fini(rcl_wait_set_t * wait_set); + +/// Retrieve the wait set's allocator. +/** + * The allocator must be an allocated rcl_allocator_t struct, as the result is + * copied into this variable. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] wait_set the handle to the wait set + * \param[out] allocator the rcl_allocator_t struct to which the result is copied + * \return #RCL_RET_OK if the allocator was successfully retrieved, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_WAIT_SET_INVALID if the wait set is invalid, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * allocator); + +/// Store a pointer to the given subscription in the next empty spot in the set. +/** + * This function does not guarantee that the subscription is not already in the + * wait set. + * + * Also add the rmw representation to the underlying rmw array and increment + * the rmw array count. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] wait_set struct in which the subscription is to be stored + * \param[in] subscription the subscription to be added to the wait set + * \param[out] index the index of the added subscription in the storage container. + * This parameter is optional and can be set to `NULL` to be ignored. + * \return #RCL_RET_OK if added successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or + * \return #RCL_RET_WAIT_SET_FULL if the subscription set is full, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_add_subscription( + rcl_wait_set_t * wait_set, + const rcl_subscription_t * subscription, + size_t * index); + +/// Remove (sets to `NULL`) all entities in the wait set. +/** + * This function should be used after passing using rcl_wait, but before + * adding new entities to the set. + * Sets all of the entries in the underlying rmw array to `NULL`, and sets the + * count in the rmw array to `0`. + * + * Calling this on an uninitialized (zero initialized) wait set will fail. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] wait_set struct to have its entities cleared + * \return #RCL_RET_OK if cleared successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_clear(rcl_wait_set_t * wait_set); + +/// Reallocate space for entities in the wait set. +/** + * This function will deallocate and reallocate the memory for all entity sets. + * + * A size of 0 will just deallocate the memory and assign `NULL` to the array. + * + * Allocation and deallocation is done with the allocator given during the + * wait set's initialization. + * + * After calling this function all values in the set will be set to `NULL`, + * effectively the same as calling rcl_wait_set_clear(). + * Similarly, the underlying rmw representation is reallocated and reset: + * all entries are set to `NULL` and the count is set to zero. + * + * If the requested size matches the current size, no allocation will be done. + * + * This can be called on an uninitialized (zero initialized) wait set. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | Yes + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[inout] wait_set struct to be resized + * \param[in] subscriptions_size a size for the new subscriptions set + * \param[in] guard_conditions_size a size for the new guard conditions set + * \param[in] timers_size a size for the new timers set + * \param[in] clients_size a size for the new clients set + * \param[in] services_size a size for the new services set + * \param[in] events_size a size for the new events set + * \return #RCL_RET_OK if resized successfully, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_BAD_ALLOC if allocating memory failed, or + * \return #RCL_RET_ERROR if an unspecified error occurs. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_resize( + rcl_wait_set_t * wait_set, + size_t subscriptions_size, + size_t guard_conditions_size, + size_t timers_size, + size_t clients_size, + size_t services_size, + size_t events_size); + +/// Store a pointer to the guard condition in the next empty spot in the set. +/** + * This function behaves exactly the same as for subscriptions. + * \see rcl_wait_set_add_subscription + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_add_guard_condition( + rcl_wait_set_t * wait_set, + const rcl_guard_condition_t * guard_condition, + size_t * index); + +/// Store a pointer to the timer in the next empty spot in the set. +/** + * This function behaves exactly the same as for subscriptions. + * \see rcl_wait_set_add_subscription + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_add_timer( + rcl_wait_set_t * wait_set, + const rcl_timer_t * timer, + size_t * index); + +/// Store a pointer to the client in the next empty spot in the set. +/** + * This function behaves exactly the same as for subscriptions. + * \see rcl_wait_set_add_subscription + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_add_client( + rcl_wait_set_t * wait_set, + const rcl_client_t * client, + size_t * index); + +/// Store a pointer to the service in the next empty spot in the set. +/** + * This function behaves exactly the same as for subscriptions. + * \see rcl_wait_set_add_subscription + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_add_service( + rcl_wait_set_t * wait_set, + const rcl_service_t * service, + size_t * index); + +/// Store a pointer to the event in the next empty spot in the set. +/** + * This function behaves exactly the same as for subscriptions. + * \see rcl_wait_set_add_subscription + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait_set_add_event( + rcl_wait_set_t * wait_set, + const rcl_event_t * event, + size_t * index); + +/// Block until the wait set is ready or until the timeout has been exceeded. +/** + * This function will collect the items in the rcl_wait_set_t and pass them + * to the underlying rmw_wait function. + * + * The items in the wait set will be either left untouched or set to `NULL` after + * this function returns. + * Items that are not `NULL` are ready, where ready means different things based + * on the type of the item. + * For subscriptions this means there may be messages that can be taken, or + * perhaps that the state of the subscriptions has changed, in which case + * rcl_take may succeed but return with taken == false. + * For guard conditions this means the guard condition was triggered. + * + * Expected usage: + * + * ```c + * #include + * + * // rcl_init() called successfully before here... + * rcl_node_t node; // initialize this, see rcl_node_init() + * rcl_subscription_t sub1; // initialize this, see rcl_subscription_init() + * rcl_subscription_t sub2; // initialize this, see rcl_subscription_init() + * rcl_guard_condition_t gc1; // initialize this, see rcl_guard_condition_init() + * rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + * rcl_ret_t ret = rcl_wait_set_init(&wait_set, 2, 1, 0, 0, 0, rcl_get_default_allocator()); + * // ... error handling + * do { + * ret = rcl_wait_set_clear(&wait_set); + * // ... error handling + * ret = rcl_wait_set_add_subscription(&wait_set, &sub1); + * // ... error handling + * ret = rcl_wait_set_add_subscription(&wait_set, &sub2); + * // ... error handling + * ret = rcl_wait_set_add_guard_condition(&wait_set, &gc1); + * // ... error handling + * ret = rcl_wait(&wait_set, RCL_MS_TO_NS(1000)); // 1000ms == 1s, passed as ns + * if (ret == RCL_RET_TIMEOUT) { + * continue; + * } + * for (int i = 0; i < wait_set.size_of_subscriptions; ++i) { + * if (wait_set.subscriptions[i]) { + * // The subscription is ready... + * } + * } + * for (int i = 0; i < wait_set.size_of_guard_conditions; ++i) { + * if (wait_set.guard_conditions[i]) { + * // The subscription is ready... + * } + * } + * } while(check_some_condition()); + * // ... fini node, and subscriptions and guard conditions... + * ret = rcl_wait_set_fini(&wait_set); + * // ... error handling + * ``` + * + * The wait set struct must be allocated, initialized, and should have been + * cleared and then filled with items, e.g. subscriptions and guard conditions. + * Passing a wait set with no wait-able items in it will fail. + * `NULL` items in the sets are ignored, e.g. it is valid to have as input: + * - `subscriptions[0]` = valid pointer + * - `subscriptions[1]` = `NULL` + * - `subscriptions[2]` = valid pointer + * - `size_of_subscriptions` = 3 + * Passing an uninitialized (zero initialized) wait set struct will fail. + * Passing a wait set struct with uninitialized memory is undefined behavior. + * + * The unit of timeout is nanoseconds. + * If the timeout is negative then this function will block indefinitely until + * something in the wait set is valid or it is interrupted. + * If the timeout is 0 then this function will be non-blocking; checking what's + * ready now, but not waiting if nothing is ready yet. + * If the timeout is greater than 0 then this function will return after + * that period of time has elapsed or the wait set becomes ready, which ever + * comes first. + * Passing a timeout struct with uninitialized memory is undefined behavior. + * + * This function is thread-safe for unique wait sets with unique contents. + * This function cannot operate on the same wait set in multiple threads, and + * the wait sets may not share content. + * For example, calling rcl_wait() in two threads on two different wait sets + * that both contain a single, shared guard condition is undefined behavior. + * + * \param[inout] wait_set the set of things to be waited on and to be pruned if not ready + * \param[in] timeout the duration to wait for the wait set to be ready, in nanoseconds + * \return #RCL_RET_OK something in the wait set became ready, or + * \return #RCL_RET_INVALID_ARGUMENT if any arguments are invalid, or + * \return #RCL_RET_WAIT_SET_INVALID if the wait set is zero initialized, or + * \return #RCL_RET_WAIT_SET_EMPTY if the wait set contains no items, or + * \return #RCL_RET_TIMEOUT if the timeout expired before something was ready, or + * \return #RCL_RET_ERROR an unspecified error occur. + */ +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout); + +/// Return `true` if the wait set is valid, else `false`. +/** + * A wait set is invalid if: + * - the implementation is `NULL` (rcl_wait_set_init not called or failed) + * - the wait set has been finalized with rcl_wait_set_fini + * + * Also return `false` if the wait set pointer is `NULL`. + * + *
+ * Attribute | Adherence + * ------------------ | ------------- + * Allocates Memory | No + * Thread-Safe | No + * Uses Atomics | No + * Lock-Free | Yes + * + * \param[in] wait_set the rcl_wait_set_t to be validated + * \return `true` if the wait_set is valid, otherwise `false`. + */ +RCL_PUBLIC +bool +rcl_wait_set_is_valid(const rcl_wait_set_t * wait_set); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__WAIT_H_ diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..fba9b9b --- /dev/null +++ b/package.xml @@ -0,0 +1,45 @@ + + + + rcl + 5.3.0 + The ROS client library common implementation. + This package contains an API which builds on the ROS middleware API and is optionally built upon by the other ROS client libraries. + + Ivan Paunovic + Jacob Perron + William Woodall + Apache License 2.0 + + ament_cmake_ros + + rmw + + rcl_interfaces + rcl_logging_interface + rcl_logging_spdlog + rcl_yaml_param_parser + rcutils + rmw_implementation + rosidl_runtime_c + tracetools + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + launch + launch_testing + launch_testing_ament_cmake + mimick_vendor + osrf_testing_tools_cpp + rcpputils + rmw + rmw_implementation_cmake + test_msgs + + rcl_logging_packages + + + ament_cmake + + diff --git a/rcl-extras.cmake b/rcl-extras.cmake new file mode 100644 index 0000000..24e83b5 --- /dev/null +++ b/rcl-extras.cmake @@ -0,0 +1,15 @@ +# 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("${rcl_DIR}/rcl_set_symbol_visibility_hidden.cmake") diff --git a/src/rcl/arguments.c b/src/rcl/arguments.c new file mode 100644 index 0000000..8c3e7c0 --- /dev/null +++ b/src/rcl/arguments.c @@ -0,0 +1,2079 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \cond INTERNAL // Internal Doxygen documentation + +#include "rcl/arguments.h" + +#include +#include + +#include "./arguments_impl.h" +#include "./remap_impl.h" +#include "rcl/error_handling.h" +#include "rcl/lexer_lookahead.h" +#include "rcl/validate_topic_name.h" +#include "rcl_yaml_param_parser/parser.h" +#include "rcl_yaml_param_parser/types.h" +#include "rcutils/allocator.h" +#include "rcutils/error_handling.h" +#include "rcutils/format_string.h" +#include "rcutils/logging.h" +#include "rcutils/logging_macros.h" +#include "rcutils/strdup.h" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// Parse an argument that may or may not be a remap rule. +/** + * \param[in] arg the argument to parse + * \param[in] allocator an allocator to use + * \param[in,out] output_rule input a zero intialized rule, output a fully initialized one + * \return RCL_RET_OK if a valid rule was parsed, or + * \return RCL_RET_INVALID_REMAP_RULE if the argument is not a valid rule, or + * \return RCL_RET_BAD_ALLOC if an allocation failed, or + * \return RLC_RET_ERROR if an unspecified error occurred. + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_remap_rule( + const char * arg, + rcl_allocator_t allocator, + rcl_remap_t * output_rule); + +/// Parse an argument that may or may not be a param rule. +/** + * \param[in] arg the argument to parse + * \param[in,out] params param overrides structure to populate. + * This structure must have been initialized by the caller. + * \return RCL_RET_OK if a valid rule was parsed, or + * \return RCL_RET_INVALID_ARGUMENT if an argument is invalid, or + * \return RCL_RET_INVALID_PARAM_RULE if the argument is not a valid rule, or + * \return RCL_RET_BAD_ALLOC if an allocation failed, or + * \return RLC_RET_ERROR if an unspecified error occurred. + */ +rcl_ret_t +_rcl_parse_param_rule( + const char * arg, + rcl_params_t * params); + +rcl_ret_t +rcl_arguments_get_param_files( + const rcl_arguments_t * arguments, + rcl_allocator_t allocator, + char *** parameter_files) +{ + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(arguments, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(arguments->impl, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_files, RCL_RET_INVALID_ARGUMENT); + *(parameter_files) = allocator.allocate( + sizeof(char *) * arguments->impl->num_param_files_args, allocator.state); + if (NULL == *parameter_files) { + return RCL_RET_BAD_ALLOC; + } + for (int i = 0; i < arguments->impl->num_param_files_args; ++i) { + (*parameter_files)[i] = rcutils_strdup(arguments->impl->parameter_files[i], allocator); + if (NULL == (*parameter_files)[i]) { + // deallocate allocated memory + for (int r = i; r >= 0; --r) { + allocator.deallocate((*parameter_files)[r], allocator.state); + } + allocator.deallocate((*parameter_files), allocator.state); + (*parameter_files) = NULL; + return RCL_RET_BAD_ALLOC; + } + } + return RCL_RET_OK; +} + +int +rcl_arguments_get_param_files_count( + const rcl_arguments_t * args) +{ + if (NULL == args || NULL == args->impl) { + return -1; + } + return args->impl->num_param_files_args; +} + +rcl_ret_t +rcl_arguments_get_param_overrides( + const rcl_arguments_t * arguments, + rcl_params_t ** parameter_overrides) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(arguments, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(arguments->impl, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(parameter_overrides, RCL_RET_INVALID_ARGUMENT); + + if (NULL != *parameter_overrides) { + RCL_SET_ERROR_MSG("Output parameter override pointer is not null. May leak memory."); + return RCL_RET_INVALID_ARGUMENT; + } + *parameter_overrides = NULL; + if (NULL != arguments->impl->parameter_overrides) { + *parameter_overrides = rcl_yaml_node_struct_copy(arguments->impl->parameter_overrides); + if (NULL == *parameter_overrides) { + return RCL_RET_BAD_ALLOC; + } + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_arguments_get_log_levels( + const rcl_arguments_t * arguments, + rcl_log_levels_t * log_levels) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(arguments, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(arguments->impl, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &arguments->impl->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + return rcl_log_levels_copy(&arguments->impl->log_levels, log_levels); +} + +/// Parse an argument that may or may not be a log level rule. +/** + * \param[in] arg the argument to parse + * \param[in,out] log_levels parsed a default logger level or a logger setting + * \return RCL_RET_OK if a valid log level was parsed, or + * \return RCL_RET_INVALID_LOG_LEVEL_RULE if the argument is not a valid rule, or + * \return RCL_RET_BAD_ALLOC if an allocation failed, or + * \return RLC_RET_ERROR if an unspecified error occurred. + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_log_level( + const char * arg, + rcl_log_levels_t * log_levels); + +/// Parse an argument that may or may not be a log configuration file. +/** + * \param[in] arg the argument to parse + * \param[in] allocator an allocator to use + * \param[in,out] log_config_file parsed log configuration file + * \return RCL_RET_OK if a valid log config file was parsed, or + * \return RCL_RET_BAD_ALLOC if an allocation failed, or + * \return RLC_RET_ERROR if an unspecified error occurred. + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_external_log_config_file( + const char * arg, + rcl_allocator_t allocator, + char ** log_config_file); + +/// Parse an argument that may or may not be a parameter file. +/** + * The syntax of the file name is not validated. + * \param[in] arg the argument to parse + * \param[in] allocator an allocator to use + * \param[in] params points to the populated parameter struct + * \param[in,out] param_file string that could be a parameter file name + * \return RCL_RET_OK if the rule was parsed correctly, or + * \return RCL_RET_BAD_ALLOC if an allocation failed, or + * \return RLC_RET_ERROR if an unspecified error occurred. + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_param_file( + const char * arg, + rcl_allocator_t allocator, + rcl_params_t * params, + char ** param_file); + +/// Parse an enclave argument. +/** + * \param[in] arg the argument to parse + * \param[in] allocator an allocator to use + * \param[in,out] enclave parsed security enclave + * \return RCL_RET_OK if a valid security enclave was parsed, or + * \return RCL_RET_BAD_ALLOC if an allocation failed, or + * \return RLC_RET_ERROR if an unspecified error occurred. + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_enclave( + const char * arg, + rcl_allocator_t allocator, + char ** enclave); + +#define RCL_ENABLE_FLAG_PREFIX "--enable-" +#define RCL_DISABLE_FLAG_PREFIX "--disable-" + +/// Parse a bool argument that may or may not be for the provided key rule. +/** + * \param[in] arg the argument to parse + * \param[in] key the key for the argument to parse. Should be a null terminated string + * \param[in,out] value parsed boolean value + * \return RCL_RET_OK if the bool argument was parsed successfully, or + * \return RLC_RET_ERROR if an unspecified error occurred. + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_disabling_flag( + const char * arg, + const char * key, + bool * value); + +/// Allocate and zero initialize arguments impl and. +/** + * \param[out] args target arguments to set impl + * \param[in] allocator an allocator to use + * \return RCL_RET_OK if a valid rule was parsed, or + * \return RCL_RET_BAD_ALLOC if an allocation failed + */ +rcl_ret_t +_rcl_allocate_initialized_arguments_impl(rcl_arguments_t * args, rcl_allocator_t * allocator); + +rcl_ret_t +rcl_parse_arguments( + int argc, + const char * const * argv, + rcl_allocator_t allocator, + rcl_arguments_t * args_output) +{ + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + if (argc < 0) { + RCL_SET_ERROR_MSG("Argument count cannot be negative"); + return RCL_RET_INVALID_ARGUMENT; + } else if (argc > 0) { + RCL_CHECK_ARGUMENT_FOR_NULL(argv, RCL_RET_INVALID_ARGUMENT); + } + RCL_CHECK_ARGUMENT_FOR_NULL(args_output, RCL_RET_INVALID_ARGUMENT); + + if (args_output->impl != NULL) { + RCL_SET_ERROR_MSG("Parse output is not zero-initialized"); + return RCL_RET_INVALID_ARGUMENT; + } + + rcl_ret_t ret; + rcl_ret_t fail_ret; + + ret = _rcl_allocate_initialized_arguments_impl(args_output, &allocator); + if (RCL_RET_OK != ret) { + return ret; + } + rcl_arguments_impl_t * args_impl = args_output->impl; + + if (argc == 0) { + // there are no arguments to parse + return RCL_RET_OK; + } + + // over-allocate arrays to match the number of arguments + args_impl->remap_rules = allocator.allocate(sizeof(rcl_remap_t) * argc, allocator.state); + if (NULL == args_impl->remap_rules) { + ret = RCL_RET_BAD_ALLOC; + goto fail; + } + + args_impl->parameter_overrides = rcl_yaml_node_struct_init(allocator); + if (NULL == args_impl->parameter_overrides) { + ret = RCL_RET_BAD_ALLOC; + goto fail; + } + + args_impl->parameter_files = allocator.allocate(sizeof(char *) * argc, allocator.state); + if (NULL == args_impl->parameter_files) { + ret = RCL_RET_BAD_ALLOC; + goto fail; + } + args_impl->unparsed_ros_args = allocator.allocate(sizeof(int) * argc, allocator.state); + if (NULL == args_impl->unparsed_ros_args) { + ret = RCL_RET_BAD_ALLOC; + goto fail; + } + args_impl->unparsed_args = allocator.allocate(sizeof(int) * argc, allocator.state); + if (NULL == args_impl->unparsed_args) { + ret = RCL_RET_BAD_ALLOC; + goto fail; + } + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + ret = rcl_log_levels_init(&log_levels, &allocator, argc); + if (ret != RCL_RET_OK) { + goto fail; + } + args_impl->log_levels = log_levels; + + bool parsing_ros_args = false; + for (int i = 0; i < argc; ++i) { + if (parsing_ros_args) { + // Ignore ROS specific arguments flags + if (strcmp(RCL_ROS_ARGS_FLAG, argv[i]) == 0) { + continue; + } + + // Check for ROS specific arguments explicit end token + if (strcmp(RCL_ROS_ARGS_EXPLICIT_END_TOKEN, argv[i]) == 0) { + parsing_ros_args = false; + continue; + } + + // Attempt to parse argument as parameter override flag + if (strcmp(RCL_PARAM_FLAG, argv[i]) == 0 || strcmp(RCL_SHORT_PARAM_FLAG, argv[i]) == 0) { + if (i + 1 < argc) { + // Attempt to parse next argument as parameter override rule + if (RCL_RET_OK == _rcl_parse_param_rule(argv[i + 1], args_impl->parameter_overrides)) { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Got param override rule : %s\n", argv[i + 1]); + ++i; // Skip flag here, for loop will skip rule. + continue; + } + rcl_error_string_t prev_error_string = rcl_get_error_string(); + rcl_reset_error(); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse parameter override rule: '%s %s'. Error: %s", argv[i], argv[i + 1], + prev_error_string.str); + } else { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse trailing %s flag. No parameter override rule found.", argv[i]); + } + ret = RCL_RET_INVALID_ROS_ARGS; + goto fail; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Arg %d (%s) is not a %s nor a %s flag.", + i, argv[i], RCL_PARAM_FLAG, RCL_SHORT_PARAM_FLAG); + + // Attempt to parse argument as remap rule flag + if (strcmp(RCL_REMAP_FLAG, argv[i]) == 0 || strcmp(RCL_SHORT_REMAP_FLAG, argv[i]) == 0) { + if (i + 1 < argc) { + // Attempt to parse next argument as remap rule + rcl_remap_t * rule = &(args_impl->remap_rules[args_impl->num_remap_rules]); + *rule = rcl_get_zero_initialized_remap(); + if (RCL_RET_OK == _rcl_parse_remap_rule(argv[i + 1], allocator, rule)) { + ++(args_impl->num_remap_rules); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Got remap rule : %s\n", argv[i + 1]); + ++i; // Skip flag here, for loop will skip rule. + continue; + } + rcl_error_string_t prev_error_string = rcl_get_error_string(); + rcl_reset_error(); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse remap rule: '%s %s'. Error: %s", argv[i], argv[i + 1], + prev_error_string.str); + } else { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse trailing %s flag. No remap rule found.", argv[i]); + } + ret = RCL_RET_INVALID_ROS_ARGS; + goto fail; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Arg %d (%s) is not a %s nor a %s flag.", + i, argv[i], RCL_REMAP_FLAG, RCL_SHORT_REMAP_FLAG); + + // Attempt to parse argument as parameter file rule + if (strcmp(RCL_PARAM_FILE_FLAG, argv[i]) == 0) { + if (i + 1 < argc) { + // Attempt to parse next argument as parameter file rule + args_impl->parameter_files[args_impl->num_param_files_args] = NULL; + if ( + RCL_RET_OK == _rcl_parse_param_file( + argv[i + 1], allocator, args_impl->parameter_overrides, + &args_impl->parameter_files[args_impl->num_param_files_args])) + { + ++(args_impl->num_param_files_args); + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, + "Got params file : %s\ntotal num param files %d", + args_impl->parameter_files[args_impl->num_param_files_args - 1], + args_impl->num_param_files_args); + ++i; // Skip flag here, for loop will skip rule. + continue; + } + rcl_error_string_t prev_error_string = rcl_get_error_string(); + rcl_reset_error(); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse params file: '%s %s'. Error: %s", argv[i], argv[i + 1], + prev_error_string.str); + } else { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse trailing %s flag. No file path provided.", argv[i]); + } + ret = RCL_RET_INVALID_ROS_ARGS; + goto fail; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Arg %d (%s) is not a %s flag.", + i, argv[i], RCL_PARAM_FILE_FLAG); + + // Attempt to parse argument as log level configuration + if (strcmp(RCL_LOG_LEVEL_FLAG, argv[i]) == 0) { + if (i + 1 < argc) { + if (RCL_RET_OK == + _rcl_parse_log_level(argv[i + 1], &args_impl->log_levels)) + { + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Got log level: %s\n", argv[i + 1]); + ++i; // Skip flag here, for loop will skip value. + continue; + } + rcl_error_string_t prev_error_string = rcl_get_error_string(); + rcl_reset_error(); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse log level: '%s %s'. Error: %s", argv[i], argv[i + 1], + prev_error_string.str); + } else { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse trailing log level flag: '%s'. No log level provided.", argv[i]); + } + ret = RCL_RET_INVALID_ROS_ARGS; + goto fail; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Arg %d (%s) is not a %s flag.", + i, argv[i], RCL_LOG_LEVEL_FLAG); + + // Attempt to parse argument as log configuration file + if (strcmp(RCL_EXTERNAL_LOG_CONFIG_FLAG, argv[i]) == 0) { + if (i + 1 < argc) { + if (NULL != args_impl->external_log_config_file) { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Overriding log configuration file : %s\n", + args_impl->external_log_config_file); + allocator.deallocate(args_impl->external_log_config_file, allocator.state); + args_impl->external_log_config_file = NULL; + } + if (RCL_RET_OK == _rcl_parse_external_log_config_file( + argv[i + 1], allocator, &args_impl->external_log_config_file)) + { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Got log configuration file : %s\n", + args_impl->external_log_config_file); + ++i; // Skip flag here, for loop will skip value. + continue; + } + rcl_error_string_t prev_error_string = rcl_get_error_string(); + rcl_reset_error(); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse log configuration file: '%s %s'. Error: %s", argv[i], argv[i + 1], + prev_error_string.str); + } else { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse trailing %s flag. No file path provided.", argv[i]); + } + ret = RCL_RET_INVALID_ROS_ARGS; + goto fail; + } + + // Attempt to parse argument as a security enclave + if (strcmp(RCL_ENCLAVE_FLAG, argv[i]) == 0 || strcmp(RCL_SHORT_ENCLAVE_FLAG, argv[i]) == 0) { + if (i + 1 < argc) { + if (NULL != args_impl->enclave) { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Overriding security enclave : %s\n", + args_impl->enclave); + allocator.deallocate(args_impl->enclave, allocator.state); + args_impl->enclave = NULL; + } + if (RCL_RET_OK == _rcl_parse_enclave( + argv[i + 1], allocator, &args_impl->enclave)) + { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Got enclave: %s\n", + args_impl->enclave); + ++i; // Skip flag here, for loop will skip value. + continue; + } + rcl_error_string_t prev_error_string = rcl_get_error_string(); + rcl_reset_error(); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse enclave name: '%s %s'. Error: %s", argv[i], argv[i + 1], + prev_error_string.str); + } else { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Couldn't parse trailing %s flag. No enclave path provided.", argv[i]); + } + ret = RCL_RET_INVALID_ROS_ARGS; + goto fail; + } + + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Arg %d (%s) is not a %s flag.", + i, argv[i], RCL_EXTERNAL_LOG_CONFIG_FLAG); + + // Attempt to parse --enable/disable-stdout-logs flag + ret = _rcl_parse_disabling_flag( + argv[i], RCL_LOG_STDOUT_FLAG_SUFFIX, &args_impl->log_stdout_disabled); + if (RCL_RET_OK == ret) { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Disable log stdout ? %s\n", + args_impl->log_stdout_disabled ? "true" : "false"); + continue; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, + "Couldn't parse arg %d (%s) as %s%s or %s%s flag. Error: %s", + i, argv[i], RCL_ENABLE_FLAG_PREFIX, RCL_LOG_STDOUT_FLAG_SUFFIX, + RCL_DISABLE_FLAG_PREFIX, RCL_LOG_STDOUT_FLAG_SUFFIX, rcl_get_error_string().str); + rcl_reset_error(); + + // Attempt to parse --enable/disable-rosout-logs flag + ret = _rcl_parse_disabling_flag( + argv[i], RCL_LOG_ROSOUT_FLAG_SUFFIX, &args_impl->log_rosout_disabled); + if (RCL_RET_OK == ret) { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Disable log rosout ? %s\n", + args_impl->log_rosout_disabled ? "true" : "false"); + continue; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, + "Couldn't parse arg %d (%s) as %s%s or %s%s flag. Error: %s", + i, argv[i], RCL_ENABLE_FLAG_PREFIX, RCL_LOG_ROSOUT_FLAG_SUFFIX, + RCL_DISABLE_FLAG_PREFIX, RCL_LOG_ROSOUT_FLAG_SUFFIX, rcl_get_error_string().str); + rcl_reset_error(); + + // Attempt to parse --enable/disable-external-lib-logs flag + ret = _rcl_parse_disabling_flag( + argv[i], RCL_LOG_EXT_LIB_FLAG_SUFFIX, &args_impl->log_ext_lib_disabled); + if (RCL_RET_OK == ret) { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Disable log external lib ? %s\n", + args_impl->log_ext_lib_disabled ? "true" : "false"); + continue; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, + "Couldn't parse arg %d (%s) as %s%s or %s%s flag. Error: %s", + i, argv[i], RCL_ENABLE_FLAG_PREFIX, RCL_LOG_EXT_LIB_FLAG_SUFFIX, + RCL_DISABLE_FLAG_PREFIX, RCL_LOG_EXT_LIB_FLAG_SUFFIX, rcl_get_error_string().str); + rcl_reset_error(); + + // Argument is an unknown ROS specific argument + args_impl->unparsed_ros_args[args_impl->num_unparsed_ros_args] = i; + ++(args_impl->num_unparsed_ros_args); + } else { + // Check for ROS specific arguments flags + if (strcmp(RCL_ROS_ARGS_FLAG, argv[i]) == 0) { + parsing_ros_args = true; + continue; + } + + // Attempt to parse argument as remap rule in its deprecated form + rcl_remap_t * rule = &(args_impl->remap_rules[args_impl->num_remap_rules]); + *rule = rcl_get_zero_initialized_remap(); + if (RCL_RET_OK == _rcl_parse_remap_rule(argv[i], allocator, rule)) { + RCUTILS_LOG_WARN_NAMED( + ROS_PACKAGE_NAME, + "Found remap rule '%s'. This syntax is deprecated. Use '%s %s %s' instead.", + argv[i], RCL_ROS_ARGS_FLAG, RCL_REMAP_FLAG, argv[i]); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Got remap rule : %s\n", argv[i + 1]); + ++(args_impl->num_remap_rules); + continue; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, + "Couldn't parse arg %d (%s) as a remap rule in its deprecated form. Error: %s", + i, argv[i], rcl_get_error_string().str); + rcl_reset_error(); + + // Argument is not a ROS specific argument + args_impl->unparsed_args[args_impl->num_unparsed_args] = i; + ++(args_impl->num_unparsed_args); + } + } + + // Shrink remap_rules array to match number of successfully parsed rules + if (0 == args_impl->num_remap_rules) { + // No remap rules + allocator.deallocate(args_impl->remap_rules, allocator.state); + args_impl->remap_rules = NULL; + } else if (args_impl->num_remap_rules < argc) { + rcl_remap_t * new_remap_rules = allocator.reallocate( + args_impl->remap_rules, + sizeof(rcl_remap_t) * args_impl->num_remap_rules, + &allocator); + if (NULL == new_remap_rules) { + ret = RCL_RET_BAD_ALLOC; + goto fail; + } + args_impl->remap_rules = new_remap_rules; + } + + // Shrink Parameter files + if (0 == args_impl->num_param_files_args) { + allocator.deallocate(args_impl->parameter_files, allocator.state); + args_impl->parameter_files = NULL; + } else if (args_impl->num_param_files_args < argc) { + char ** new_parameter_files = allocator.reallocate( + args_impl->parameter_files, + sizeof(char *) * args_impl->num_param_files_args, + &allocator); + if (NULL == new_parameter_files) { + ret = RCL_RET_BAD_ALLOC; + goto fail; + } + args_impl->parameter_files = new_parameter_files; + } + + // Drop parameter overrides if none was found. + if (0U == args_impl->parameter_overrides->num_nodes) { + rcl_yaml_node_struct_fini(args_impl->parameter_overrides); + args_impl->parameter_overrides = NULL; + } + + // Shrink unparsed_ros_args + if (0 == args_impl->num_unparsed_ros_args) { + // No unparsed ros args + allocator.deallocate(args_impl->unparsed_ros_args, allocator.state); + args_impl->unparsed_ros_args = NULL; + } else if (args_impl->num_unparsed_ros_args < argc) { + args_impl->unparsed_ros_args = rcutils_reallocf( + args_impl->unparsed_ros_args, sizeof(int) * args_impl->num_unparsed_ros_args, &allocator); + if (NULL == args_impl->unparsed_ros_args) { + ret = RCL_RET_BAD_ALLOC; + goto fail; + } + } + + // Shrink unparsed_args + if (0 == args_impl->num_unparsed_args) { + // No unparsed args + allocator.deallocate(args_impl->unparsed_args, allocator.state); + args_impl->unparsed_args = NULL; + } else if (args_impl->num_unparsed_args < argc) { + args_impl->unparsed_args = rcutils_reallocf( + args_impl->unparsed_args, sizeof(int) * args_impl->num_unparsed_args, &allocator); + if (NULL == args_impl->unparsed_args) { + ret = RCL_RET_BAD_ALLOC; + goto fail; + } + } + + // Shrink logger settings of log levels + ret = rcl_log_levels_shrink_to_size(&args_impl->log_levels); + if (ret != RCL_RET_OK) { + goto fail; + } + + return RCL_RET_OK; +fail: + fail_ret = ret; + if (NULL != args_impl) { + ret = rcl_arguments_fini(args_output); + if (RCL_RET_OK != ret) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini arguments after earlier failure"); + } + } + return fail_ret; +} + +int +rcl_arguments_get_count_unparsed( + const rcl_arguments_t * args) +{ + if (NULL == args || NULL == args->impl) { + return -1; + } + return args->impl->num_unparsed_args; +} + +rcl_ret_t +rcl_arguments_get_unparsed( + const rcl_arguments_t * args, + rcl_allocator_t allocator, + int ** output_unparsed_indices) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(args, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(args->impl, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(output_unparsed_indices, RCL_RET_INVALID_ARGUMENT); + + *output_unparsed_indices = NULL; + if (args->impl->num_unparsed_args) { + *output_unparsed_indices = allocator.allocate( + sizeof(int) * args->impl->num_unparsed_args, allocator.state); + if (NULL == *output_unparsed_indices) { + return RCL_RET_BAD_ALLOC; + } + for (int i = 0; i < args->impl->num_unparsed_args; ++i) { + (*output_unparsed_indices)[i] = args->impl->unparsed_args[i]; + } + } + return RCL_RET_OK; +} + +int +rcl_arguments_get_count_unparsed_ros( + const rcl_arguments_t * args) +{ + if (NULL == args || NULL == args->impl) { + return -1; + } + return args->impl->num_unparsed_ros_args; +} + +rcl_ret_t +rcl_arguments_get_unparsed_ros( + const rcl_arguments_t * args, + rcl_allocator_t allocator, + int ** output_unparsed_ros_indices) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(args, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(args->impl, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(output_unparsed_ros_indices, RCL_RET_INVALID_ARGUMENT); + + *output_unparsed_ros_indices = NULL; + if (args->impl->num_unparsed_ros_args) { + *output_unparsed_ros_indices = allocator.allocate( + sizeof(int) * args->impl->num_unparsed_ros_args, allocator.state); + if (NULL == *output_unparsed_ros_indices) { + return RCL_RET_BAD_ALLOC; + } + for (int i = 0; i < args->impl->num_unparsed_ros_args; ++i) { + (*output_unparsed_ros_indices)[i] = args->impl->unparsed_ros_args[i]; + } + } + return RCL_RET_OK; +} + +rcl_arguments_t +rcl_get_zero_initialized_arguments(void) +{ + static rcl_arguments_t default_arguments = { + .impl = NULL + }; + return default_arguments; +} + +rcl_ret_t +rcl_remove_ros_arguments( + const char * const * argv, + const rcl_arguments_t * args, + rcl_allocator_t allocator, + int * nonros_argc, + const char *** nonros_argv) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(args, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(nonros_argc, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(nonros_argv, RCL_RET_INVALID_ARGUMENT); + if (NULL != *nonros_argv) { + RCL_SET_ERROR_MSG("Output nonros_argv pointer is not null. May leak memory."); + return RCL_RET_INVALID_ARGUMENT; + } + + *nonros_argc = rcl_arguments_get_count_unparsed(args); + if (*nonros_argc < 0) { + RCL_SET_ERROR_MSG("Failed to get unparsed non ROS specific arguments count."); + return RCL_RET_INVALID_ARGUMENT; + } else if (*nonros_argc > 0) { + RCL_CHECK_ARGUMENT_FOR_NULL(argv, RCL_RET_INVALID_ARGUMENT); + } + + *nonros_argv = NULL; + if (0 == *nonros_argc) { + return RCL_RET_OK; + } + + int * unparsed_indices = NULL; + rcl_ret_t ret = rcl_arguments_get_unparsed(args, allocator, &unparsed_indices); + + if (RCL_RET_OK != ret) { + return ret; + } + + size_t alloc_size = sizeof(char *) * *nonros_argc; + *nonros_argv = allocator.allocate(alloc_size, allocator.state); + if (NULL == *nonros_argv) { + allocator.deallocate(unparsed_indices, allocator.state); + return RCL_RET_BAD_ALLOC; + } + for (int i = 0; i < *nonros_argc; ++i) { + (*nonros_argv)[i] = argv[unparsed_indices[i]]; + } + + allocator.deallocate(unparsed_indices, allocator.state); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_arguments_copy( + const rcl_arguments_t * args, + rcl_arguments_t * args_out) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + + RCL_CHECK_ARGUMENT_FOR_NULL(args, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(args->impl, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(args_out, RCL_RET_INVALID_ARGUMENT); + if (NULL != args_out->impl) { + RCL_SET_ERROR_MSG("args_out must be zero initialized"); + return RCL_RET_INVALID_ARGUMENT; + } + + rcl_allocator_t allocator = args->impl->allocator; + + rcl_ret_t ret = _rcl_allocate_initialized_arguments_impl(args_out, &allocator); + if (RCL_RET_OK != ret) { + return ret; + } + + if (args->impl->num_unparsed_args) { + // Copy unparsed args + args_out->impl->unparsed_args = allocator.allocate( + sizeof(int) * args->impl->num_unparsed_args, allocator.state); + if (NULL == args_out->impl->unparsed_args) { + if (RCL_RET_OK != rcl_arguments_fini(args_out)) { + RCL_SET_ERROR_MSG("Error while finalizing arguments due to another error"); + } + return RCL_RET_BAD_ALLOC; + } + for (int i = 0; i < args->impl->num_unparsed_args; ++i) { + args_out->impl->unparsed_args[i] = args->impl->unparsed_args[i]; + } + args_out->impl->num_unparsed_args = args->impl->num_unparsed_args; + } + + if (args->impl->num_unparsed_ros_args) { + // Copy unparsed ROS args + args_out->impl->unparsed_ros_args = allocator.allocate( + sizeof(int) * args->impl->num_unparsed_ros_args, allocator.state); + if (NULL == args_out->impl->unparsed_ros_args) { + if (RCL_RET_OK != rcl_arguments_fini(args_out)) { + RCL_SET_ERROR_MSG("Error while finalizing arguments due to another error"); + } + return RCL_RET_BAD_ALLOC; + } + for (int i = 0; i < args->impl->num_unparsed_ros_args; ++i) { + args_out->impl->unparsed_ros_args[i] = args->impl->unparsed_ros_args[i]; + } + args_out->impl->num_unparsed_ros_args = args->impl->num_unparsed_ros_args; + } + + if (args->impl->num_remap_rules) { + // Copy remap rules + args_out->impl->remap_rules = allocator.allocate( + sizeof(rcl_remap_t) * args->impl->num_remap_rules, allocator.state); + if (NULL == args_out->impl->remap_rules) { + if (RCL_RET_OK != rcl_arguments_fini(args_out)) { + RCL_SET_ERROR_MSG("Error while finalizing arguments due to another error"); + } + return RCL_RET_BAD_ALLOC; + } + for (int i = 0; i < args->impl->num_remap_rules; ++i) { + args_out->impl->remap_rules[i] = rcl_get_zero_initialized_remap(); + ret = rcl_remap_copy( + &(args->impl->remap_rules[i]), &(args_out->impl->remap_rules[i])); + if (RCL_RET_OK != ret) { + if (RCL_RET_OK != rcl_arguments_fini(args_out)) { + RCL_SET_ERROR_MSG("Error while finalizing arguments due to another error"); + } + return ret; + } + ++(args_out->impl->num_remap_rules); + } + } + + // Copy parameter rules + if (args->impl->parameter_overrides) { + args_out->impl->parameter_overrides = + rcl_yaml_node_struct_copy(args->impl->parameter_overrides); + } + + // Copy parameter files + if (args->impl->num_param_files_args) { + args_out->impl->parameter_files = allocator.zero_allocate( + args->impl->num_param_files_args, sizeof(char *), allocator.state); + if (NULL == args_out->impl->parameter_files) { + if (RCL_RET_OK != rcl_arguments_fini(args_out)) { + RCL_SET_ERROR_MSG("Error while finalizing arguments due to another error"); + } + return RCL_RET_BAD_ALLOC; + } + for (int i = 0; i < args->impl->num_param_files_args; ++i) { + args_out->impl->parameter_files[i] = + rcutils_strdup(args->impl->parameter_files[i], allocator); + if (NULL == args_out->impl->parameter_files[i]) { + if (RCL_RET_OK != rcl_arguments_fini(args_out)) { + RCL_SET_ERROR_MSG("Error while finalizing arguments due to another error"); + } + return RCL_RET_BAD_ALLOC; + } + ++(args_out->impl->num_param_files_args); + } + } + char * enclave_copy = rcutils_strdup(args->impl->enclave, allocator); + if (args->impl->enclave && !enclave_copy) { + if (RCL_RET_OK != rcl_arguments_fini(args_out)) { + RCL_SET_ERROR_MSG("Error while finalizing arguments due to another error"); + } else { + RCL_SET_ERROR_MSG("Error while copying enclave argument"); + } + return RCL_RET_BAD_ALLOC; + } + args_out->impl->enclave = enclave_copy; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_arguments_fini( + rcl_arguments_t * args) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(args, RCL_RET_INVALID_ARGUMENT); + if (args->impl) { + rcl_ret_t ret = RCL_RET_OK; + if (args->impl->remap_rules) { + for (int i = 0; i < args->impl->num_remap_rules; ++i) { + rcl_ret_t remap_ret = rcl_remap_fini(&(args->impl->remap_rules[i])); + if (remap_ret != RCL_RET_OK) { + ret = remap_ret; + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Failed to finalize remap rule while finalizing arguments. Continuing..."); + } + } + args->impl->allocator.deallocate(args->impl->remap_rules, args->impl->allocator.state); + args->impl->remap_rules = NULL; + args->impl->num_remap_rules = 0; + } + + rcl_ret_t log_levels_ret = rcl_log_levels_fini(&args->impl->log_levels); + if (log_levels_ret != RCL_RET_OK) { + ret = log_levels_ret; + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Failed to finalize log levels while finalizing arguments. Continuing..."); + } + + args->impl->allocator.deallocate(args->impl->unparsed_args, args->impl->allocator.state); + args->impl->num_unparsed_args = 0; + args->impl->unparsed_args = NULL; + + args->impl->allocator.deallocate(args->impl->unparsed_ros_args, args->impl->allocator.state); + args->impl->num_unparsed_ros_args = 0; + args->impl->unparsed_ros_args = NULL; + + if (args->impl->parameter_overrides) { + rcl_yaml_node_struct_fini(args->impl->parameter_overrides); + args->impl->parameter_overrides = NULL; + } + + if (args->impl->parameter_files) { + for (int p = 0; p < args->impl->num_param_files_args; ++p) { + args->impl->allocator.deallocate( + args->impl->parameter_files[p], args->impl->allocator.state); + } + args->impl->allocator.deallocate(args->impl->parameter_files, args->impl->allocator.state); + args->impl->num_param_files_args = 0; + args->impl->parameter_files = NULL; + } + args->impl->allocator.deallocate(args->impl->enclave, args->impl->allocator.state); + + if (NULL != args->impl->external_log_config_file) { + args->impl->allocator.deallocate( + args->impl->external_log_config_file, args->impl->allocator.state); + args->impl->external_log_config_file = NULL; + } + + args->impl->allocator.deallocate(args->impl, args->impl->allocator.state); + args->impl = NULL; + return ret; + } + RCL_SET_ERROR_MSG("rcl_arguments_t finalized twice"); + return RCL_RET_ERROR; +} + +/// Parses a fully qualified namespace for a namespace replacement rule (ex: `/foo/bar`) +/** + * \sa _rcl_parse_remap_begin_remap_rule() + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_remap_fully_qualified_namespace( + rcl_lexer_lookahead2_t * lex_lookahead) +{ + rcl_ret_t ret; + + // Check arguments sanity + assert(NULL != lex_lookahead); + + // Must have at least one Forward slash / + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_FORWARD_SLASH, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + return RCL_RET_INVALID_REMAP_RULE; + } + + // repeated tokens and slashes (allow trailing slash, but don't require it) + while (true) { + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_TOKEN, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + rcl_reset_error(); + break; + } + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_FORWARD_SLASH, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + rcl_reset_error(); + break; + } + } + return RCL_RET_OK; +} + +/// Parse either a token or a backreference (ex: `bar`, or `\7`). +/** + * \sa _rcl_parse_remap_begin_remap_rule() + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_remap_replacement_token(rcl_lexer_lookahead2_t * lex_lookahead) +{ + rcl_ret_t ret; + rcl_lexeme_t lexeme; + + // Check arguments sanity + assert(NULL != lex_lookahead); + + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + + if ( + RCL_LEXEME_BR1 == lexeme || RCL_LEXEME_BR2 == lexeme || RCL_LEXEME_BR3 == lexeme || + RCL_LEXEME_BR4 == lexeme || RCL_LEXEME_BR5 == lexeme || RCL_LEXEME_BR6 == lexeme || + RCL_LEXEME_BR7 == lexeme || RCL_LEXEME_BR8 == lexeme || RCL_LEXEME_BR9 == lexeme) + { + RCL_SET_ERROR_MSG("Backreferences are not implemented"); + return RCL_RET_ERROR; + } else if (RCL_LEXEME_TOKEN == lexeme) { + ret = rcl_lexer_lookahead2_accept(lex_lookahead, NULL, NULL); + } else { + ret = RCL_RET_INVALID_REMAP_RULE; + } + + return ret; +} + +/// Parse the replacement side of a name remapping rule (ex: `bar/\1/foo`). +/** + * \sa _rcl_parse_remap_begin_remap_rule() + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_remap_replacement_name( + rcl_lexer_lookahead2_t * lex_lookahead, + rcl_remap_t * rule) +{ + rcl_ret_t ret; + rcl_lexeme_t lexeme; + + // Check arguments sanity + assert(NULL != lex_lookahead); + assert(NULL != rule); + + const char * replacement_start = rcl_lexer_lookahead2_get_text(lex_lookahead); + if (NULL == replacement_start) { + RCL_SET_ERROR_MSG("failed to get start of replacement"); + return RCL_RET_ERROR; + } + + // private name (~/...) or fully qualified name (/...) ? + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + if (RCL_LEXEME_TILDE_SLASH == lexeme || RCL_LEXEME_FORWARD_SLASH == lexeme) { + ret = rcl_lexer_lookahead2_accept(lex_lookahead, NULL, NULL); + } + if (RCL_RET_OK != ret) { + return ret; + } + + // token ( '/' token )* + ret = _rcl_parse_remap_replacement_token(lex_lookahead); + if (RCL_RET_OK != ret) { + return ret; + } + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + while (RCL_LEXEME_EOF != lexeme) { + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_FORWARD_SLASH, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + return RCL_RET_INVALID_REMAP_RULE; + } + ret = _rcl_parse_remap_replacement_token(lex_lookahead); + if (RCL_RET_OK != ret) { + return ret; + } + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + } + + // Copy replacement into rule + const char * replacement_end = rcl_lexer_lookahead2_get_text(lex_lookahead); + size_t length = (size_t)(replacement_end - replacement_start); + rule->impl->replacement = rcutils_strndup( + replacement_start, length, rule->impl->allocator); + if (NULL == rule->impl->replacement) { + RCL_SET_ERROR_MSG("failed to copy replacement"); + return RCL_RET_BAD_ALLOC; + } + + return RCL_RET_OK; +} + +/// Parse either a resource name token or a wildcard (ex: `foobar`, or `*`, or `**`). +/** + * \sa _rcl_parse_resource_match() + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_resource_match_token(rcl_lexer_lookahead2_t * lex_lookahead) +{ + rcl_ret_t ret; + rcl_lexeme_t lexeme; + + // Check arguments sanity + assert(NULL != lex_lookahead); + + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + + if (RCL_LEXEME_TOKEN == lexeme) { + ret = rcl_lexer_lookahead2_accept(lex_lookahead, NULL, NULL); + } else if (RCL_LEXEME_WILD_ONE == lexeme) { + RCL_SET_ERROR_MSG("Wildcard '*' is not implemented"); + return RCL_RET_ERROR; + } else if (RCL_LEXEME_WILD_MULTI == lexeme) { + RCL_SET_ERROR_MSG("Wildcard '**' is not implemented"); + return RCL_RET_ERROR; + } else { + RCL_SET_ERROR_MSG("Expecting token or wildcard"); + ret = RCL_RET_WRONG_LEXEME; + } + + return ret; +} + +/// Parse a resource name match side of a rule (ex: `rostopic://foo`) +/** + * \sa _rcl_parse_remap_match_name() + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_resource_match( + rcl_lexer_lookahead2_t * lex_lookahead, + rcl_allocator_t allocator, + char ** resource_match) +{ + rcl_ret_t ret; + rcl_lexeme_t lexeme; + + // Check arguments sanity + assert(NULL != lex_lookahead); + assert(rcutils_allocator_is_valid(&allocator)); + assert(NULL != resource_match); + assert(NULL == *resource_match); + + const char * match_start = rcl_lexer_lookahead2_get_text(lex_lookahead); + if (NULL == match_start) { + RCL_SET_ERROR_MSG("failed to get start of match"); + return RCL_RET_ERROR; + } + + // private name (~/...) or fully qualified name (/...) ? + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + if (RCL_LEXEME_TILDE_SLASH == lexeme || RCL_LEXEME_FORWARD_SLASH == lexeme) { + ret = rcl_lexer_lookahead2_accept(lex_lookahead, NULL, NULL); + if (RCL_RET_OK != ret) { + return ret; + } + } + + // token ( '/' token )* + ret = _rcl_parse_resource_match_token(lex_lookahead); + if (RCL_RET_OK != ret) { + return ret; + } + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + while (RCL_LEXEME_SEPARATOR != lexeme) { + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_FORWARD_SLASH, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + return RCL_RET_INVALID_REMAP_RULE; + } + ret = _rcl_parse_resource_match_token(lex_lookahead); + if (RCL_RET_OK != ret) { + return ret; + } + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + } + + // Copy match into rule + const char * match_end = rcl_lexer_lookahead2_get_text(lex_lookahead); + const size_t length = (size_t)(match_end - match_start); + *resource_match = rcutils_strndup(match_start, length, allocator); + if (NULL == *resource_match) { + RCL_SET_ERROR_MSG("failed to copy match"); + return RCL_RET_BAD_ALLOC; + } + + return RCL_RET_OK; +} + +RCL_LOCAL +rcl_ret_t +_rcl_parse_param_name_token(rcl_lexer_lookahead2_t * lex_lookahead) +{ + rcl_ret_t ret; + rcl_lexeme_t lexeme; + + // Check arguments sanity + assert(NULL != lex_lookahead); + + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + if (RCL_LEXEME_TOKEN != lexeme && RCL_LEXEME_FORWARD_SLASH != lexeme) { + if (RCL_LEXEME_WILD_ONE == lexeme) { + RCL_SET_ERROR_MSG("Wildcard '*' is not implemented"); + return RCL_RET_ERROR; + } else if (RCL_LEXEME_WILD_MULTI == lexeme) { + RCL_SET_ERROR_MSG("Wildcard '**' is not implemented"); + return RCL_RET_ERROR; + } else { + RCL_SET_ERROR_MSG("Expecting token or wildcard"); + return RCL_RET_WRONG_LEXEME; + } + } + do { + ret = rcl_lexer_lookahead2_accept(lex_lookahead, NULL, NULL); + if (RCL_RET_OK != ret) { + return ret; + } + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + } while (RCL_LEXEME_TOKEN == lexeme || RCL_LEXEME_FORWARD_SLASH == lexeme); + return RCL_RET_OK; +} + +/// Parse a parameter name in a parameter override rule (ex: `foo.bar`) +/** + * \sa _rcl_parse_param_rule() + */ +// TODO(hidmic): remove when parameter names are standardized to use slashes +// in lieu of dots. +RCL_LOCAL +rcl_ret_t +_rcl_parse_param_name( + rcl_lexer_lookahead2_t * lex_lookahead, + rcl_allocator_t allocator, + char ** param_name) +{ + rcl_ret_t ret; + rcl_lexeme_t lexeme; + + // Check arguments sanity + assert(NULL != lex_lookahead); + assert(rcutils_allocator_is_valid(&allocator)); + assert(NULL != param_name); + assert(NULL == *param_name); + + const char * name_start = rcl_lexer_lookahead2_get_text(lex_lookahead); + if (NULL == name_start) { + RCL_SET_ERROR_MSG("failed to get start of param name"); + return RCL_RET_ERROR; + } + + // token ( '.' token )* + ret = _rcl_parse_param_name_token(lex_lookahead); + if (RCL_RET_OK != ret) { + return ret; + } + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + while (RCL_LEXEME_SEPARATOR != lexeme) { + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_DOT, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + return RCL_RET_INVALID_REMAP_RULE; + } + ret = _rcl_parse_param_name_token(lex_lookahead); + if (RCL_RET_OK != ret) { + return ret; + } + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + } + + // Copy param name + const char * name_end = rcl_lexer_lookahead2_get_text(lex_lookahead); + const size_t length = (size_t)(name_end - name_start); + *param_name = rcutils_strndup(name_start, length, allocator); + if (NULL == *param_name) { + RCL_SET_ERROR_MSG("failed to copy param name"); + return RCL_RET_BAD_ALLOC; + } + + return RCL_RET_OK; +} + + +/// Parse the match side of a name remapping rule (ex: `rostopic://foo`) +/** + * \sa _rcl_parse_remap_begin_remap_rule() + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_remap_match_name( + rcl_lexer_lookahead2_t * lex_lookahead, + rcl_remap_t * rule) +{ + rcl_ret_t ret; + rcl_lexeme_t lexeme; + + // Check arguments sanity + assert(NULL != lex_lookahead); + assert(NULL != rule); + + // rostopic:// rosservice:// + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + if (RCL_LEXEME_URL_SERVICE == lexeme) { + rule->impl->type = RCL_SERVICE_REMAP; + ret = rcl_lexer_lookahead2_accept(lex_lookahead, NULL, NULL); + } else if (RCL_LEXEME_URL_TOPIC == lexeme) { + rule->impl->type = RCL_TOPIC_REMAP; + ret = rcl_lexer_lookahead2_accept(lex_lookahead, NULL, NULL); + } else { + rule->impl->type = (RCL_TOPIC_REMAP | RCL_SERVICE_REMAP); + } + if (RCL_RET_OK != ret) { + return ret; + } + + ret = _rcl_parse_resource_match( + lex_lookahead, rule->impl->allocator, &rule->impl->match); + if (RCL_RET_WRONG_LEXEME == ret) { + ret = RCL_RET_INVALID_REMAP_RULE; + } + return ret; +} + +/// Parse a name remapping rule (ex: `rostopic:///foo:=bar`). +/** + * \sa _rcl_parse_remap_begin_remap_rule() + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_remap_name_remap( + rcl_lexer_lookahead2_t * lex_lookahead, + rcl_remap_t * rule) +{ + rcl_ret_t ret; + + // Check arguments sanity + assert(NULL != lex_lookahead); + assert(NULL != rule); + + // match + ret = _rcl_parse_remap_match_name(lex_lookahead, rule); + if (RCL_RET_OK != ret) { + return ret; + } + // := + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_SEPARATOR, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + return RCL_RET_INVALID_REMAP_RULE; + } + // replacement + ret = _rcl_parse_remap_replacement_name(lex_lookahead, rule); + if (RCL_RET_OK != ret) { + return ret; + } + + return RCL_RET_OK; +} + +/// Parse a namespace replacement rule (ex: `__ns:=/new/ns`). +/** + * \sa _rcl_parse_remap_begin_remap_rule() + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_remap_namespace_replacement( + rcl_lexer_lookahead2_t * lex_lookahead, + rcl_remap_t * rule) +{ + rcl_ret_t ret; + + // Check arguments sanity + assert(NULL != lex_lookahead); + assert(NULL != rule); + + // __ns + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_NS, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + return RCL_RET_INVALID_REMAP_RULE; + } + // := + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_SEPARATOR, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + return RCL_RET_INVALID_REMAP_RULE; + } + // /foo/bar + const char * ns_start = rcl_lexer_lookahead2_get_text(lex_lookahead); + if (NULL == ns_start) { + RCL_SET_ERROR_MSG("failed to get start of namespace"); + return RCL_RET_ERROR; + } + ret = _rcl_parse_remap_fully_qualified_namespace(lex_lookahead); + if (RCL_RET_OK != ret) { + if (RCL_RET_INVALID_REMAP_RULE == ret) { + // The name didn't start with a leading forward slash + RCUTILS_LOG_WARN_NAMED( + ROS_PACKAGE_NAME, "Namespace not remapped to a fully qualified name (found: %s)", ns_start); + } + return ret; + } + // There should be nothing left + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_EOF, NULL, NULL); + if (RCL_RET_OK != ret) { + // The name must have started with a leading forward slash but had an otherwise invalid format + RCUTILS_LOG_WARN_NAMED( + ROS_PACKAGE_NAME, "Namespace not remapped to a fully qualified name (found: %s)", ns_start); + return ret; + } + + // Copy namespace into rule + const char * ns_end = rcl_lexer_lookahead2_get_text(lex_lookahead); + size_t length = (size_t)(ns_end - ns_start); + rule->impl->replacement = rcutils_strndup(ns_start, length, rule->impl->allocator); + if (NULL == rule->impl->replacement) { + RCL_SET_ERROR_MSG("failed to copy namespace"); + return RCL_RET_BAD_ALLOC; + } + + rule->impl->type = RCL_NAMESPACE_REMAP; + return RCL_RET_OK; +} + +/// Parse a nodename replacement rule (ex: `__node:=new_name` or `__name:=new_name`). +/** + * \sa _rcl_parse_remap_begin_remap_rule() + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_remap_nodename_replacement( + rcl_lexer_lookahead2_t * lex_lookahead, + rcl_remap_t * rule) +{ + rcl_ret_t ret; + const char * node_name; + size_t length; + + // Check arguments sanity + assert(NULL != lex_lookahead); + assert(NULL != rule); + + // __node + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_NODE, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + return RCL_RET_INVALID_REMAP_RULE; + } + // := + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_SEPARATOR, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + return RCL_RET_INVALID_REMAP_RULE; + } + // new_node_name + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_TOKEN, &node_name, &length); + if (RCL_RET_WRONG_LEXEME == ret) { + node_name = rcl_lexer_lookahead2_get_text(lex_lookahead); + RCUTILS_LOG_WARN_NAMED( + ROS_PACKAGE_NAME, "Node name not remapped to invalid name: '%s'", node_name); + return RCL_RET_INVALID_REMAP_RULE; + } + if (RCL_RET_OK != ret) { + return ret; + } + // copy the node name into the replacement side of the rule + rule->impl->replacement = rcutils_strndup(node_name, length, rule->impl->allocator); + if (NULL == rule->impl->replacement) { + RCL_SET_ERROR_MSG("failed to allocate node name"); + return RCL_RET_BAD_ALLOC; + } + + rule->impl->type = RCL_NODENAME_REMAP; + return RCL_RET_OK; +} + +/// Parse a nodename prefix including trailing colon (ex: `node_name:`). +RCL_LOCAL +rcl_ret_t +_rcl_parse_nodename_prefix( + rcl_lexer_lookahead2_t * lex_lookahead, + rcl_allocator_t allocator, + char ** node_name) +{ + size_t length = 0; + const char * token = NULL; + + // Check arguments sanity + assert(NULL != lex_lookahead); + assert(rcutils_allocator_is_valid(&allocator)); + assert(NULL != node_name); + assert(NULL == *node_name); + + // Expect a token and a colon + rcl_ret_t ret = + rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_TOKEN, &token, &length); + if (RCL_RET_OK != ret) { + return ret; + } + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_COLON, NULL, NULL); + if (RCL_RET_OK != ret) { + return ret; + } + + // Copy the node name + *node_name = rcutils_strndup(token, length, allocator); + if (NULL == *node_name) { + RCL_SET_ERROR_MSG("failed to allocate node name"); + return RCL_RET_BAD_ALLOC; + } + + return RCL_RET_OK; +} + +/// Parse a nodename prefix for a remap rule. +/** + * \sa _rcl_parse_nodename_prefix() + * \sa _rcl_parse_remap_begin_remap_rule() + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_remap_nodename_prefix( + rcl_lexer_lookahead2_t * lex_lookahead, + rcl_remap_t * rule) +{ + // Check arguments sanity + assert(NULL != lex_lookahead); + assert(NULL != rule); + + rcl_ret_t ret = _rcl_parse_nodename_prefix( + lex_lookahead, rule->impl->allocator, &rule->impl->node_name); + if (RCL_RET_WRONG_LEXEME == ret) { + ret = RCL_RET_INVALID_REMAP_RULE; + } + return ret; +} + +/// Start recursive descent parsing of a remap rule. +/** + * \param[in] lex_lookahead a lookahead(2) buffer for the parser to use. + * \param[in,out] rule input a zero intialized rule, output a fully initialized one. + * \return RCL_RET_OK if a valid rule was parsed, or + * \return RCL_RET_INVALID_REMAP_RULE if the argument is not a valid rule, or + * \return RCL_RET_BAD_ALLOC if an allocation failed, or + * \return RLC_RET_ERROR if an unspecified error occurred. + */ +RCL_LOCAL +rcl_ret_t +_rcl_parse_remap_begin_remap_rule( + rcl_lexer_lookahead2_t * lex_lookahead, + rcl_remap_t * rule) +{ + rcl_ret_t ret; + rcl_lexeme_t lexeme1; + rcl_lexeme_t lexeme2; + + // Check arguments sanity + assert(NULL != lex_lookahead); + assert(NULL != rule); + + // Check for optional nodename prefix + ret = rcl_lexer_lookahead2_peek2(lex_lookahead, &lexeme1, &lexeme2); + if (RCL_RET_OK != ret) { + return ret; + } + if (RCL_LEXEME_TOKEN == lexeme1 && RCL_LEXEME_COLON == lexeme2) { + ret = _rcl_parse_remap_nodename_prefix(lex_lookahead, rule); + if (RCL_RET_OK != ret) { + return ret; + } + } + + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme1); + if (RCL_RET_OK != ret) { + return ret; + } + + // What type of rule is this (node name replacement, namespace replacement, or name remap)? + if (RCL_LEXEME_NODE == lexeme1) { + ret = _rcl_parse_remap_nodename_replacement(lex_lookahead, rule); + if (RCL_RET_OK != ret) { + return ret; + } + } else if (RCL_LEXEME_NS == lexeme1) { + ret = _rcl_parse_remap_namespace_replacement(lex_lookahead, rule); + if (RCL_RET_OK != ret) { + return ret; + } + } else { + ret = _rcl_parse_remap_name_remap(lex_lookahead, rule); + if (RCL_RET_OK != ret) { + return ret; + } + } + + // Make sure all characters in string have been consumed + ret = rcl_lexer_lookahead2_expect(lex_lookahead, RCL_LEXEME_EOF, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + return RCL_RET_INVALID_REMAP_RULE; + } + return ret; +} + +RCL_LOCAL +rcl_ret_t +_rcl_parse_log_level_name( + rcl_lexer_lookahead2_t * lex_lookahead, + rcl_allocator_t * allocator, + char ** logger_name) +{ + rcl_lexeme_t lexeme; + + // Check arguments sanity + assert(NULL != lex_lookahead); + assert(rcutils_allocator_is_valid(allocator)); + assert(NULL != logger_name); + assert(NULL == *logger_name); + + const char * name_start = rcl_lexer_lookahead2_get_text(lex_lookahead); + if (NULL == name_start) { + RCL_SET_ERROR_MSG("failed to get start of logger name"); + return RCL_RET_ERROR; + } + + rcl_ret_t ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + + while (RCL_LEXEME_SEPARATOR != lexeme) { + ret = rcl_lexer_lookahead2_expect(lex_lookahead, lexeme, NULL, NULL); + if (RCL_RET_OK != ret) { + return ret; + } + + ret = rcl_lexer_lookahead2_peek(lex_lookahead, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + + if (lexeme == RCL_LEXEME_EOF) { + ret = RCL_RET_INVALID_LOG_LEVEL_RULE; + return ret; + } + } + + // Copy logger name + const char * name_end = rcl_lexer_lookahead2_get_text(lex_lookahead); + const size_t length = (size_t)(name_end - name_start); + *logger_name = rcutils_strndup(name_start, length, *allocator); + if (NULL == *logger_name) { + RCL_SET_ERROR_MSG("failed to copy logger name"); + return RCL_RET_BAD_ALLOC; + } + + return RCL_RET_OK; +} + +rcl_ret_t +_rcl_parse_log_level( + const char * arg, + rcl_log_levels_t * log_levels) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(arg, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels->logger_settings, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t * allocator = &log_levels->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret = RCL_RET_OK; + char * logger_name = NULL; + int level = 0; + rcutils_ret_t rcutils_ret = RCUTILS_RET_OK; + + rcl_lexer_lookahead2_t lex_lookahead = rcl_get_zero_initialized_lexer_lookahead2(); + + ret = rcl_lexer_lookahead2_init(&lex_lookahead, arg, *allocator); + if (RCL_RET_OK != ret) { + return ret; + } + + ret = _rcl_parse_log_level_name(&lex_lookahead, allocator, &logger_name); + if (RCL_RET_OK == ret) { + if (strlen(logger_name) == 0) { + RCL_SET_ERROR_MSG("Argument has an invalid logger item that name is empty"); + ret = RCL_RET_INVALID_LOG_LEVEL_RULE; + goto cleanup; + } + + ret = rcl_lexer_lookahead2_expect(&lex_lookahead, RCL_LEXEME_SEPARATOR, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + ret = RCL_RET_INVALID_LOG_LEVEL_RULE; + goto cleanup; + } + + const char * level_token; + size_t level_token_length; + ret = rcl_lexer_lookahead2_expect( + &lex_lookahead, RCL_LEXEME_TOKEN, &level_token, &level_token_length); + if (RCL_RET_WRONG_LEXEME == ret) { + ret = RCL_RET_INVALID_LOG_LEVEL_RULE; + goto cleanup; + } + + ret = rcl_lexer_lookahead2_expect(&lex_lookahead, RCL_LEXEME_EOF, NULL, NULL); + if (RCL_RET_OK != ret) { + ret = RCL_RET_INVALID_LOG_LEVEL_RULE; + goto cleanup; + } + + rcutils_ret = rcutils_logging_severity_level_from_string( + level_token, *allocator, &level); + if (RCUTILS_RET_OK == rcutils_ret) { + ret = rcl_log_levels_add_logger_setting( + log_levels, logger_name, (rcl_log_severity_t)level); + if (ret != RCL_RET_OK) { + goto cleanup; + } + } + } else { + rcutils_ret = rcutils_logging_severity_level_from_string( + arg, *allocator, &level); + if (RCUTILS_RET_OK == rcutils_ret) { + if (log_levels->default_logger_level != (rcl_log_severity_t)level) { + if (log_levels->default_logger_level != RCUTILS_LOG_SEVERITY_UNSET) { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Minimum default log level will be replaced from %d to %d", + log_levels->default_logger_level, level); + } + log_levels->default_logger_level = (rcl_log_severity_t)level; + } + ret = RCL_RET_OK; + } + } + + if (RCUTILS_RET_OK != rcutils_ret) { + RCL_SET_ERROR_MSG("Argument does not use a valid severity level"); + ret = RCL_RET_ERROR; + } + +cleanup: + if (logger_name) { + allocator->deallocate(logger_name, allocator->state); + } + rcl_ret_t rv = rcl_lexer_lookahead2_fini(&lex_lookahead); + if (RCL_RET_OK != rv) { + if (RCL_RET_OK != ret) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini lookahead2 after error occurred"); + } else { + ret = rv; + } + } + + return ret; +} + +rcl_ret_t +_rcl_parse_remap_rule( + const char * arg, + rcl_allocator_t allocator, + rcl_remap_t * output_rule) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(arg, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(output_rule, RCL_RET_INVALID_ARGUMENT); + + output_rule->impl = + allocator.allocate(sizeof(rcl_remap_impl_t), allocator.state); + if (NULL == output_rule->impl) { + return RCL_RET_BAD_ALLOC; + } + output_rule->impl->allocator = allocator; + output_rule->impl->type = RCL_UNKNOWN_REMAP; + output_rule->impl->node_name = NULL; + output_rule->impl->match = NULL; + output_rule->impl->replacement = NULL; + + rcl_lexer_lookahead2_t lex_lookahead = rcl_get_zero_initialized_lexer_lookahead2(); + rcl_ret_t ret = rcl_lexer_lookahead2_init(&lex_lookahead, arg, allocator); + + if (RCL_RET_OK == ret) { + ret = _rcl_parse_remap_begin_remap_rule(&lex_lookahead, output_rule); + + rcl_ret_t fini_ret = rcl_lexer_lookahead2_fini(&lex_lookahead); + if (RCL_RET_OK != ret) { + if (RCL_RET_OK != fini_ret) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Failed to fini lookahead2 after error occurred"); + } + } else { + if (RCL_RET_OK == fini_ret) { + return RCL_RET_OK; + } + ret = fini_ret; + } + } + + // cleanup output rule but keep first error return code + if (RCL_RET_OK != rcl_remap_fini(output_rule)) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Failed to fini remap rule after error occurred"); + } + + return ret; +} + +rcl_ret_t +_rcl_parse_param_rule( + const char * arg, + rcl_params_t * params) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(arg, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(params, RCL_RET_INVALID_ARGUMENT); + + rcl_lexer_lookahead2_t lex_lookahead = rcl_get_zero_initialized_lexer_lookahead2(); + + rcl_ret_t ret = rcl_lexer_lookahead2_init(&lex_lookahead, arg, params->allocator); + if (RCL_RET_OK != ret) { + return ret; + } + + rcl_lexeme_t lexeme1; + rcl_lexeme_t lexeme2; + char * node_name = NULL; + char * param_name = NULL; + + // Check for optional nodename prefix + ret = rcl_lexer_lookahead2_peek2(&lex_lookahead, &lexeme1, &lexeme2); + if (RCL_RET_OK != ret) { + goto cleanup; + } + + if (RCL_LEXEME_TOKEN == lexeme1 && RCL_LEXEME_COLON == lexeme2) { + ret = _rcl_parse_nodename_prefix(&lex_lookahead, params->allocator, &node_name); + if (RCL_RET_OK != ret) { + if (RCL_RET_WRONG_LEXEME == ret) { + ret = RCL_RET_INVALID_PARAM_RULE; + } + goto cleanup; + } + } else { + node_name = rcutils_strdup("/**", params->allocator); + if (NULL == node_name) { + ret = RCL_RET_BAD_ALLOC; + goto cleanup; + } + } + + // TODO(hidmic): switch to _rcl_parse_resource_match() when parameter names + // are standardized to use slashes in lieu of dots. + ret = _rcl_parse_param_name(&lex_lookahead, params->allocator, ¶m_name); + if (RCL_RET_OK != ret) { + if (RCL_RET_WRONG_LEXEME == ret) { + ret = RCL_RET_INVALID_PARAM_RULE; + } + goto cleanup; + } + + ret = rcl_lexer_lookahead2_expect(&lex_lookahead, RCL_LEXEME_SEPARATOR, NULL, NULL); + if (RCL_RET_WRONG_LEXEME == ret) { + ret = RCL_RET_INVALID_PARAM_RULE; + goto cleanup; + } + + const char * yaml_value = rcl_lexer_lookahead2_get_text(&lex_lookahead); + if (!rcl_parse_yaml_value(node_name, param_name, yaml_value, params)) { + ret = RCL_RET_INVALID_PARAM_RULE; + goto cleanup; + } + +cleanup: + params->allocator.deallocate(param_name, params->allocator.state); + params->allocator.deallocate(node_name, params->allocator.state); + if (RCL_RET_OK != ret) { + if (RCL_RET_OK != rcl_lexer_lookahead2_fini(&lex_lookahead)) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini lookahead2 after error occurred"); + } + } else { + ret = rcl_lexer_lookahead2_fini(&lex_lookahead); + } + return ret; +} + +rcl_ret_t +_rcl_parse_param_file( + const char * arg, + rcl_allocator_t allocator, + rcl_params_t * params, + char ** param_file) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(arg, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(params, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(param_file, RCL_RET_INVALID_ARGUMENT); + *param_file = rcutils_strdup(arg, allocator); + if (NULL == *param_file) { + RCL_SET_ERROR_MSG("Failed to allocate memory for parameters file path"); + return RCL_RET_BAD_ALLOC; + } + if (!rcl_parse_yaml_file(*param_file, params)) { + allocator.deallocate(*param_file, allocator.state); + *param_file = NULL; + // Error message already set. + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +rcl_ret_t +_rcl_parse_external_log_config_file( + const char * arg, + rcl_allocator_t allocator, + char ** log_config_file) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(arg, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(log_config_file, RCL_RET_INVALID_ARGUMENT); + + *log_config_file = rcutils_strdup(arg, allocator); + // TODO(hidmic): add file checks + if (NULL == *log_config_file) { + RCL_SET_ERROR_MSG("Failed to allocate memory for external log config file"); + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_OK; +} + +rcl_ret_t +_rcl_parse_enclave( + const char * arg, + rcl_allocator_t allocator, + char ** enclave) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(arg, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(enclave, RCL_RET_INVALID_ARGUMENT); + + *enclave = rcutils_strdup(arg, allocator); + if (NULL == *enclave) { + RCL_SET_ERROR_MSG("Failed to allocate memory for enclave name"); + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_OK; +} + +rcl_ret_t +_rcl_parse_disabling_flag( + const char * arg, + const char * suffix, + bool * disable) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(arg, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(suffix, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(disable, RCL_RET_INVALID_ARGUMENT); + + const size_t enable_prefix_len = strlen(RCL_ENABLE_FLAG_PREFIX); + if ( + strncmp(RCL_ENABLE_FLAG_PREFIX, arg, enable_prefix_len) == 0 && + strcmp(suffix, arg + enable_prefix_len) == 0) + { + *disable = false; + return RCL_RET_OK; + } + + const size_t disable_prefix_len = strlen(RCL_DISABLE_FLAG_PREFIX); + if ( + strncmp(RCL_DISABLE_FLAG_PREFIX, arg, disable_prefix_len) == 0 && + strcmp(suffix, arg + disable_prefix_len) == 0) + { + *disable = true; + return RCL_RET_OK; + } + + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Argument is not a %s%s nor a %s%s flag.", + RCL_ENABLE_FLAG_PREFIX, suffix, + RCL_DISABLE_FLAG_PREFIX, suffix); + return RCL_RET_ERROR; +} + +rcl_ret_t +_rcl_allocate_initialized_arguments_impl(rcl_arguments_t * args, rcl_allocator_t * allocator) +{ + args->impl = allocator->allocate(sizeof(rcl_arguments_impl_t), allocator->state); + if (NULL == args->impl) { + return RCL_RET_BAD_ALLOC; + } + + rcl_arguments_impl_t * args_impl = args->impl; + args_impl->num_remap_rules = 0; + args_impl->remap_rules = NULL; + args_impl->log_levels = rcl_get_zero_initialized_log_levels(); + args_impl->external_log_config_file = NULL; + args_impl->unparsed_args = NULL; + args_impl->num_unparsed_args = 0; + args_impl->unparsed_ros_args = NULL; + args_impl->num_unparsed_ros_args = 0; + args_impl->parameter_overrides = NULL; + args_impl->parameter_files = NULL; + args_impl->num_param_files_args = 0; + args_impl->log_stdout_disabled = false; + args_impl->log_rosout_disabled = false; + args_impl->log_ext_lib_disabled = false; + args_impl->enclave = NULL; + args_impl->allocator = *allocator; + + return RCL_RET_OK; +} + +#ifdef __cplusplus +} +#endif + +/// \endcond // Internal Doxygen documentation diff --git a/src/rcl/arguments_impl.h b/src/rcl/arguments_impl.h new file mode 100644 index 0000000..d3aedbb --- /dev/null +++ b/src/rcl/arguments_impl.h @@ -0,0 +1,76 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__ARGUMENTS_IMPL_H_ +#define RCL__ARGUMENTS_IMPL_H_ + +#include "rcl/arguments.h" +#include "rcl/log_level.h" +#include "rcl_yaml_param_parser/types.h" +#include "./remap_impl.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// \internal +struct rcl_arguments_impl_s +{ + /// Array of indices to unknown ROS specific arguments. + int * unparsed_ros_args; + /// Length of unparsed_ros_args. + int num_unparsed_ros_args; + + /// Array of indices to non-ROS arguments. + int * unparsed_args; + /// Length of unparsed_args. + int num_unparsed_args; + + /// Parameter override rules parsed from arguments. + rcl_params_t * parameter_overrides; + + /// Array of yaml parameter file paths + char ** parameter_files; + /// Length of parameter_files. + int num_param_files_args; + + /// Array of rules for name remapping. + rcl_remap_t * remap_rules; + /// Length of remap_rules. + int num_remap_rules; + + /// Log levels parsed from arguments. + rcl_log_levels_t log_levels; + /// A file used to configure the external logging library + char * external_log_config_file; + /// A boolean value indicating if the standard out handler should be used for log output + bool log_stdout_disabled; + /// A boolean value indicating if the rosout topic handler should be used for log output + bool log_rosout_disabled; + /// A boolean value indicating if the external lib handler should be used for log output + bool log_ext_lib_disabled; + + /// Enclave to be used. + char * enclave; + + /// Allocator used to allocate objects in this struct + rcl_allocator_t allocator; +}; + +#ifdef __cplusplus +} +#endif + +#endif // RCL__ARGUMENTS_IMPL_H_ diff --git a/src/rcl/client.c b/src/rcl/client.c new file mode 100644 index 0000000..432948d --- /dev/null +++ b/src/rcl/client.c @@ -0,0 +1,350 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/client.h" + +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/node.h" +#include "rcutils/logging_macros.h" +#include "rcutils/macros.h" +#include "rcutils/stdatomic_helper.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "tracetools/tracetools.h" + +#include "./common.h" + +struct rcl_client_impl_s +{ + rcl_client_options_t options; + rmw_qos_profile_t actual_request_publisher_qos; + rmw_qos_profile_t actual_response_subscription_qos; + rmw_client_t * rmw_handle; + atomic_int_least64_t sequence_number; +}; + +rcl_client_t +rcl_get_zero_initialized_client() +{ + static rcl_client_t null_client = {0}; + return null_client; +} + +rcl_ret_t +rcl_client_init( + rcl_client_t * client, + const rcl_node_t * node, + const rosidl_service_type_support_t * type_support, + const char * service_name, + const rcl_client_options_t * options) +{ + rcl_ret_t fail_ret = RCL_RET_ERROR; + + // check the options and allocator first, so the allocator can be passed to errors + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t * allocator = (rcl_allocator_t *)&options->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT); + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT); + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing client for service name '%s'", service_name); + if (client->impl) { + RCL_SET_ERROR_MSG("client already initialized, or memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + + // Expand the given service name. + char * remapped_service_name = NULL; + rcl_ret_t ret = rcl_node_resolve_name( + node, + service_name, + *allocator, + true, + false, + &remapped_service_name); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { + ret = RCL_RET_SERVICE_NAME_INVALID; + } else if (RCL_RET_BAD_ALLOC != ret) { + ret = RCL_RET_ERROR; + } + goto cleanup; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Expanded and remapped service name '%s'", remapped_service_name); + + // Allocate space for the implementation struct. + client->impl = (rcl_client_impl_t *)allocator->allocate( + sizeof(rcl_client_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + client->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup); + // Fill out implementation struct. + // rmw handle (create rmw client) + // TODO(wjwwood): pass along the allocator to rmw when it supports it + client->impl->rmw_handle = rmw_create_client( + rcl_node_get_rmw_handle(node), + type_support, + remapped_service_name, + &options->qos); + if (!client->impl->rmw_handle) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + goto fail; + } + + // get actual qos, and store it + rmw_ret_t rmw_ret = rmw_client_request_publisher_get_actual_qos( + client->impl->rmw_handle, + &client->impl->actual_request_publisher_qos); + + if (RMW_RET_OK != rmw_ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + goto fail; + } + + rmw_ret = rmw_client_response_subscription_get_actual_qos( + client->impl->rmw_handle, + &client->impl->actual_response_subscription_qos); + + if (RMW_RET_OK != rmw_ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + goto fail; + } + + // ROS specific namespacing conventions avoidance + // is not retrieved by get_actual_qos + client->impl->actual_request_publisher_qos.avoid_ros_namespace_conventions = + options->qos.avoid_ros_namespace_conventions; + client->impl->actual_response_subscription_qos.avoid_ros_namespace_conventions = + options->qos.avoid_ros_namespace_conventions; + + // options + client->impl->options = *options; + atomic_init(&client->impl->sequence_number, 0); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client initialized"); + ret = RCL_RET_OK; + TRACEPOINT( + rcl_client_init, + (const void *)client, + (const void *)node, + (const void *)client->impl->rmw_handle, + remapped_service_name); + goto cleanup; +fail: + if (client->impl) { + allocator->deallocate(client->impl, allocator->state); + client->impl = NULL; + } + ret = fail_ret; + // Fall through to cleanup +cleanup: + allocator->deallocate(remapped_service_name, allocator->state); + return ret; +} + +rcl_ret_t +rcl_client_fini(rcl_client_t * client, rcl_node_t * node) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing client"); + rcl_ret_t result = RCL_RET_OK; + RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT); + if (!rcl_node_is_valid_except_context(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + if (client->impl) { + rcl_allocator_t allocator = client->impl->options.allocator; + rmw_node_t * rmw_node = rcl_node_get_rmw_handle(node); + if (!rmw_node) { + return RCL_RET_INVALID_ARGUMENT; + } + rmw_ret_t ret = rmw_destroy_client(rmw_node, client->impl->rmw_handle); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + result = RCL_RET_ERROR; + } + allocator.deallocate(client->impl, allocator.state); + client->impl = NULL; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client finalized"); + return result; +} + +rcl_client_options_t +rcl_client_get_default_options() +{ + // !!! MAKE SURE THAT CHANGES TO THESE DEFAULTS ARE REFLECTED IN THE HEADER DOC STRING + static rcl_client_options_t default_options; + // Must set the allocator and qos after because they are not a compile time constant. + default_options.qos = rmw_qos_profile_services_default; + default_options.allocator = rcl_get_default_allocator(); + return default_options; +} + +const char * +rcl_client_get_service_name(const rcl_client_t * client) +{ + if (!rcl_client_is_valid(client)) { + return NULL; // error already set + } + return client->impl->rmw_handle->service_name; +} + +#define _client_get_options(client) & client->impl->options; + +const rcl_client_options_t * +rcl_client_get_options(const rcl_client_t * client) +{ + if (!rcl_client_is_valid(client)) { + return NULL; // error already set + } + return _client_get_options(client); +} + +rmw_client_t * +rcl_client_get_rmw_handle(const rcl_client_t * client) +{ + if (!rcl_client_is_valid(client)) { + return NULL; // error already set + } + return client->impl->rmw_handle; +} + +rcl_ret_t +rcl_send_request(const rcl_client_t * client, const void * ros_request, int64_t * sequence_number) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client sending service request"); + if (!rcl_client_is_valid(client)) { + return RCL_RET_CLIENT_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(ros_request, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(sequence_number, RCL_RET_INVALID_ARGUMENT); + *sequence_number = rcutils_atomic_load_int64_t(&client->impl->sequence_number); + if (rmw_send_request( + client->impl->rmw_handle, ros_request, sequence_number) != RMW_RET_OK) + { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + rcutils_atomic_exchange_int64_t(&client->impl->sequence_number, *sequence_number); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_take_response_with_info( + const rcl_client_t * client, + rmw_service_info_t * request_header, + void * ros_response) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Client taking service response"); + if (!rcl_client_is_valid(client)) { + return RCL_RET_CLIENT_INVALID; // error already set + } + + RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, RCL_RET_INVALID_ARGUMENT); + + bool taken = false; + request_header->source_timestamp = 0; + request_header->received_timestamp = 0; + if (rmw_take_response( + client->impl->rmw_handle, request_header, ros_response, &taken) != RMW_RET_OK) + { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Client take response succeeded: %s", taken ? "true" : "false"); + if (!taken) { + return RCL_RET_CLIENT_TAKE_FAILED; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_take_response( + const rcl_client_t * client, + rmw_request_id_t * request_header, + void * ros_response) +{ + rmw_service_info_t header; + header.request_id = *request_header; + rcl_ret_t ret = rcl_take_response_with_info(client, &header, ros_response); + *request_header = header.request_id; + return ret; +} + +bool +rcl_client_is_valid(const rcl_client_t * client) +{ + RCL_CHECK_FOR_NULL_WITH_MSG(client, "client pointer is invalid", return false); + RCL_CHECK_FOR_NULL_WITH_MSG( + client->impl, "client's rmw implementation is invalid", return false); + RCL_CHECK_FOR_NULL_WITH_MSG( + client->impl->rmw_handle, "client's rmw handle is invalid", return false); + return true; +} + +const rmw_qos_profile_t * +rcl_client_request_publisher_get_actual_qos(const rcl_client_t * client) +{ + if (!rcl_client_is_valid(client)) { + return NULL; + } + return &client->impl->actual_request_publisher_qos; +} + +const rmw_qos_profile_t * +rcl_client_response_subscription_get_actual_qos(const rcl_client_t * client) +{ + if (!rcl_client_is_valid(client)) { + return NULL; + } + return &client->impl->actual_response_subscription_qos; +} + +rcl_ret_t +rcl_client_set_on_new_response_callback( + const rcl_client_t * client, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_client_is_valid(client)) { + // error state already set + return RCL_RET_INVALID_ARGUMENT; + } + + return rmw_client_set_on_new_response_callback( + client->impl->rmw_handle, + callback, + user_data); +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/common.c b/src/rcl/common.c new file mode 100644 index 0000000..5e1a299 --- /dev/null +++ b/src/rcl/common.c @@ -0,0 +1,48 @@ +// Copyright 2015 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "./common.h" // NOLINT + +#include + +#include "rcl/allocator.h" +#include "rcl/error_handling.h" + +rcl_ret_t +rcl_convert_rmw_ret_to_rcl_ret(rmw_ret_t rmw_ret) +{ + switch (rmw_ret) { + case RMW_RET_OK: + return RCL_RET_OK; + case RMW_RET_INVALID_ARGUMENT: + return RCL_RET_INVALID_ARGUMENT; + case RMW_RET_BAD_ALLOC: + return RCL_RET_BAD_ALLOC; + case RMW_RET_UNSUPPORTED: + return RCL_RET_UNSUPPORTED; + case RMW_RET_NODE_NAME_NON_EXISTENT: + return RCL_RET_NODE_NAME_NON_EXISTENT; + default: + return RCL_RET_ERROR; + } +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/common.h b/src/rcl/common.h new file mode 100644 index 0000000..67534b6 --- /dev/null +++ b/src/rcl/common.h @@ -0,0 +1,33 @@ +// Copyright 2015 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 RCL__COMMON_H_ +#define RCL__COMMON_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/types.h" + +/// Convenience function for converting common rmw_ret_t return codes to rcl. +rcl_ret_t +rcl_convert_rmw_ret_to_rcl_ret(rmw_ret_t rmw_ret); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__COMMON_H_ diff --git a/src/rcl/context.c b/src/rcl/context.c new file mode 100644 index 0000000..5414cc5 --- /dev/null +++ b/src/rcl/context.c @@ -0,0 +1,186 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/context.h" + +#include + +#include "./common.h" +#include "./context_impl.h" +#include "rcutils/stdatomic_helper.h" + +rcl_context_t +rcl_get_zero_initialized_context(void) +{ + static rcl_context_t context = { + .impl = NULL, + .instance_id_storage = {0}, + }; + // this is not constexpr so it cannot be in the struct initialization + context.global_arguments = rcl_get_zero_initialized_arguments(); + // ensure assumption about static storage + static_assert( + sizeof(context.instance_id_storage) >= sizeof(atomic_uint_least64_t), + "expected rcl_context_t's instance id storage to be >= size of atomic_uint_least64_t"); + // initialize atomic + atomic_init((atomic_uint_least64_t *)(&context.instance_id_storage), 0); + return context; +} + +// See `rcl_init()` for initialization of the context. + +rcl_ret_t +rcl_context_fini(rcl_context_t * context) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(context, RCL_RET_INVALID_ARGUMENT); + if (!context->impl) { + // Context is zero-initialized + return RCL_RET_OK; + } + if (rcl_context_is_valid(context)) { + RCL_SET_ERROR_MSG("rcl_shutdown() not called on the given context"); + return RCL_RET_INVALID_ARGUMENT; + } + RCL_CHECK_ALLOCATOR_WITH_MSG( + &(context->impl->allocator), "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + return __cleanup_context(context); +} + +// See `rcl_shutdown()` for invalidation of the context. + +const rcl_init_options_t * +rcl_context_get_init_options(const rcl_context_t * context) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(context, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(context->impl, "context is zero-initialized", return NULL); + return &(context->impl->init_options); +} + +rcl_context_instance_id_t +rcl_context_get_instance_id(const rcl_context_t * context) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(context, 0); + return rcutils_atomic_load_uint64_t((atomic_uint_least64_t *)(&context->instance_id_storage)); +} + +rcl_ret_t +rcl_context_get_domain_id(rcl_context_t * context, size_t * domain_id) +{ + if (!rcl_context_is_valid(context)) { + return RCL_RET_INVALID_ARGUMENT; + } + RCL_CHECK_ARGUMENT_FOR_NULL(domain_id, RCL_RET_INVALID_ARGUMENT); + *domain_id = context->impl->rmw_context.actual_domain_id; + return RCL_RET_OK; +} + +bool +rcl_context_is_valid(const rcl_context_t * context) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(context, false); + return 0 != rcl_context_get_instance_id(context); +} + +rmw_context_t * +rcl_context_get_rmw_context(rcl_context_t * context) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(context, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(context->impl, "context is zero-initialized", return NULL); + return &(context->impl->rmw_context); +} + +rcl_ret_t +__cleanup_context(rcl_context_t * context) +{ + rcl_ret_t ret = RCL_RET_OK; + // reset the instance id to 0 to indicate "invalid" (should already be 0, but this is defensive) + rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), 0); + + // clean up global_arguments if initialized + if (NULL != context->global_arguments.impl) { + ret = rcl_arguments_fini(&(context->global_arguments)); + if (RCL_RET_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR( + "[rcl|context.c:" RCUTILS_STRINGIFY(__LINE__) + "] failed to finalize global arguments while cleaning up context, memory may be leaked: "); + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + rcl_reset_error(); + } + } + + // if impl is null, nothing else can be cleaned up + if (NULL != context->impl) { + // pull allocator out for use during deallocation + rcl_allocator_t allocator = context->impl->allocator; + + // finalize init options if valid + if (NULL != context->impl->init_options.impl) { + rcl_ret_t init_options_fini_ret = rcl_init_options_fini(&(context->impl->init_options)); + if (RCL_RET_OK != init_options_fini_ret) { + if (RCL_RET_OK == ret) { + ret = init_options_fini_ret; + } + RCUTILS_SAFE_FWRITE_TO_STDERR( + "[rcl|context.c:" RCUTILS_STRINGIFY(__LINE__) + "] failed to finalize init options while cleaning up context, memory may be leaked: "); + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + rcl_reset_error(); + } + } + + // clean up rmw_context + if (NULL != context->impl->rmw_context.implementation_identifier) { + rmw_ret_t rmw_context_fini_ret = rmw_context_fini(&(context->impl->rmw_context)); + if (RMW_RET_OK != rmw_context_fini_ret) { + if (RCL_RET_OK == ret) { + ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_context_fini_ret); + } + RCUTILS_SAFE_FWRITE_TO_STDERR( + "[rcl|context.c:" RCUTILS_STRINGIFY(__LINE__) + "] failed to finalize rmw context while cleaning up context, memory may be leaked: "); + RCUTILS_SAFE_FWRITE_TO_STDERR(rcutils_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + rcutils_reset_error(); + } + } + + // clean up copy of argv if valid + if (NULL != context->impl->argv) { + int64_t i; + for (i = 0; i < context->impl->argc; ++i) { + if (NULL != context->impl->argv[i]) { + allocator.deallocate(context->impl->argv[i], allocator.state); + } + } + allocator.deallocate(context->impl->argv, allocator.state); + } + allocator.deallocate(context->impl, allocator.state); + } // if (NULL != context->impl) + + // zero-initialize the context + *context = rcl_get_zero_initialized_context(); + + return ret; +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/context_impl.h b/src/rcl/context_impl.h new file mode 100644 index 0000000..10c9e82 --- /dev/null +++ b/src/rcl/context_impl.h @@ -0,0 +1,51 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__CONTEXT_IMPL_H_ +#define RCL__CONTEXT_IMPL_H_ + +#include "rcl/context.h" +#include "rcl/error_handling.h" + +#include "./init_options_impl.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// \internal +struct rcl_context_impl_s +{ + /// Allocator used during init and shutdown. + rcl_allocator_t allocator; + /// Copy of init options given during init. + rcl_init_options_t init_options; + /// Length of argv (may be `0`). + int64_t argc; + /// Copy of argv used during init (may be `NULL`). + char ** argv; + /// rmw context. + rmw_context_t rmw_context; +}; + +RCL_LOCAL +rcl_ret_t +__cleanup_context(rcl_context_t * context); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__CONTEXT_IMPL_H_ diff --git a/src/rcl/domain_id.c b/src/rcl/domain_id.c new file mode 100644 index 0000000..b3d08a6 --- /dev/null +++ b/src/rcl/domain_id.c @@ -0,0 +1,59 @@ +// 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 "rcl/domain_id.h" + +#include +#include + +#include "rcutils/env.h" + +#include "rcl/error_handling.h" +#include "rcl/types.h" + +const char * const RCL_DOMAIN_ID_ENV_VAR = "ROS_DOMAIN_ID"; + +rcl_ret_t +rcl_get_default_domain_id(size_t * domain_id) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + const char * ros_domain_id = NULL; + const char * get_env_error_str = NULL; + + RCL_CHECK_ARGUMENT_FOR_NULL(domain_id, RCL_RET_INVALID_ARGUMENT); + + get_env_error_str = rcutils_get_env(RCL_DOMAIN_ID_ENV_VAR, &ros_domain_id); + if (NULL != get_env_error_str) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Error getting env var '" RCUTILS_STRINGIFY(RCL_DOMAIN_ID_ENV_VAR) "': %s\n", + get_env_error_str); + return RCL_RET_ERROR; + } + if (ros_domain_id && strcmp(ros_domain_id, "") != 0) { + char * end = NULL; + unsigned long number = strtoul(ros_domain_id, &end, 0); // NOLINT(runtime/int) + if (number == 0UL && *end != '\0') { + RCL_SET_ERROR_MSG("ROS_DOMAIN_ID is not an integral number"); + return RCL_RET_ERROR; + } + if ((number == ULONG_MAX && errno == ERANGE) || number > SIZE_MAX) { + RCL_SET_ERROR_MSG("ROS_DOMAIN_ID is out of range"); + return RCL_RET_ERROR; + } + *domain_id = (size_t)number; + } + return RCL_RET_OK; +} diff --git a/src/rcl/event.c b/src/rcl/event.c new file mode 100644 index 0000000..6196442 --- /dev/null +++ b/src/rcl/event.c @@ -0,0 +1,240 @@ +// 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/event.h" + +#include + +#include "rcl/error_handling.h" +#include "rcl/expand_topic_name.h" +#include "rcl/remap.h" +#include "rcutils/allocator.h" +#include "rcutils/logging_macros.h" +#include "rmw/error_handling.h" +#include "rmw/validate_full_topic_name.h" +#include "rmw/event.h" + +#include "./common.h" +#include "./event_impl.h" +#include "./publisher_impl.h" +#include "./subscription_impl.h" + +rcl_event_t +rcl_get_zero_initialized_event() +{ + static rcl_event_t null_event = {0}; + return null_event; +} + +rcl_ret_t +rcl_publisher_event_init( + rcl_event_t * event, + const rcl_publisher_t * publisher, + const rcl_publisher_event_type_t event_type) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(event, RCL_RET_EVENT_INVALID); + // Check publisher and allocator first, so allocator can be used with errors. + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t * allocator = &publisher->impl->options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + rmw_event_type_t rmw_event_type = RMW_EVENT_INVALID; + switch (event_type) { + case RCL_PUBLISHER_OFFERED_DEADLINE_MISSED: + rmw_event_type = RMW_EVENT_OFFERED_DEADLINE_MISSED; + break; + case RCL_PUBLISHER_LIVELINESS_LOST: + rmw_event_type = RMW_EVENT_LIVELINESS_LOST; + break; + case RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS: + rmw_event_type = RMW_EVENT_OFFERED_QOS_INCOMPATIBLE; + break; + default: + RCL_SET_ERROR_MSG("Event type for publisher not supported"); + return RCL_RET_INVALID_ARGUMENT; + } + + // Allocate space for the implementation struct. + event->impl = (rcl_event_impl_t *) allocator->allocate( + sizeof(rcl_event_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + event->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + + event->impl->rmw_handle = rmw_get_zero_initialized_event(); + event->impl->allocator = *allocator; + + rmw_ret_t ret = rmw_publisher_event_init( + &event->impl->rmw_handle, + publisher->impl->rmw_handle, + rmw_event_type); + if (ret != RMW_RET_OK) { + goto fail; + } + + return RCL_RET_OK; +fail: + allocator->deallocate(event->impl, allocator->state); + event->impl = NULL; + return rcl_convert_rmw_ret_to_rcl_ret(ret); +} + +rcl_ret_t +rcl_subscription_event_init( + rcl_event_t * event, + const rcl_subscription_t * subscription, + const rcl_subscription_event_type_t event_type) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(event, RCL_RET_EVENT_INVALID); + // Check subscription and allocator first, so allocator can be used with errors. + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t * allocator = &subscription->impl->options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + rmw_event_type_t rmw_event_type = RMW_EVENT_INVALID; + switch (event_type) { + case RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED: + rmw_event_type = RMW_EVENT_REQUESTED_DEADLINE_MISSED; + break; + case RCL_SUBSCRIPTION_LIVELINESS_CHANGED: + rmw_event_type = RMW_EVENT_LIVELINESS_CHANGED; + break; + case RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS: + rmw_event_type = RMW_EVENT_REQUESTED_QOS_INCOMPATIBLE; + break; + case RCL_SUBSCRIPTION_MESSAGE_LOST: + rmw_event_type = RMW_EVENT_MESSAGE_LOST; + break; + default: + RCL_SET_ERROR_MSG("Event type for subscription not supported"); + return RCL_RET_INVALID_ARGUMENT; + } + + // Allocate space for the implementation struct. + event->impl = (rcl_event_impl_t *) allocator->allocate( + sizeof(rcl_event_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + event->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + + event->impl->rmw_handle = rmw_get_zero_initialized_event(); + event->impl->allocator = *allocator; + + rmw_ret_t ret = rmw_subscription_event_init( + &event->impl->rmw_handle, + subscription->impl->rmw_handle, + rmw_event_type); + if (ret != RMW_RET_OK) { + goto fail; + } + + return RCL_RET_OK; +fail: + allocator->deallocate(event->impl, allocator->state); + event->impl = NULL; + return rcl_convert_rmw_ret_to_rcl_ret(ret); +} + +rcl_ret_t +rcl_take_event( + const rcl_event_t * event, + void * event_info) +{ + bool taken = false; + if (!rcl_event_is_valid(event)) { + return RCL_RET_EVENT_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(event_info, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t ret = rmw_take_event(&event->impl->rmw_handle, event_info, &taken); + if (RMW_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + if (!taken) { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "take_event request complete, unable to take event"); + return RCL_RET_EVENT_TAKE_FAILED; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "take_event request success"); + return rcl_convert_rmw_ret_to_rcl_ret(ret); +} + +rcl_ret_t +rcl_event_fini(rcl_event_t * event) +{ + rcl_ret_t result = RCL_RET_OK; + RCL_CHECK_ARGUMENT_FOR_NULL(event, RCL_RET_EVENT_INVALID); + + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing event"); + if (NULL != event->impl) { + rcl_allocator_t allocator = event->impl->allocator; + rmw_ret_t ret = rmw_event_fini(&event->impl->rmw_handle); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + result = rcl_convert_rmw_ret_to_rcl_ret(ret); + } + allocator.deallocate(event->impl, allocator.state); + event->impl = NULL; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Event finalized"); + + return result; +} + +rmw_event_t * +rcl_event_get_rmw_handle(const rcl_event_t * event) +{ + if (!rcl_event_is_valid(event)) { + return NULL; // error already set + } else { + return &event->impl->rmw_handle; + } +} + +bool +rcl_event_is_valid(const rcl_event_t * event) +{ + RCL_CHECK_FOR_NULL_WITH_MSG(event, "event pointer is invalid", return false); + RCL_CHECK_FOR_NULL_WITH_MSG(event->impl, "event's implementation is invalid", return false); + if (event->impl->rmw_handle.event_type == RMW_EVENT_INVALID) { + RCUTILS_SET_ERROR_MSG("event's implementation not init"); + return false; + } + RCUTILS_CHECK_ALLOCATOR_WITH_MSG( + &event->impl->allocator, "not valid allocator", return false); + return true; +} + +rcl_ret_t +rcl_event_set_callback( + const rcl_event_t * event, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_event_is_valid(event)) { + // error state already set + return RCL_RET_INVALID_ARGUMENT; + } + + return rmw_event_set_callback( + &event->impl->rmw_handle, + callback, + user_data); +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/event_impl.h b/src/rcl/event_impl.h new file mode 100644 index 0000000..09456dd --- /dev/null +++ b/src/rcl/event_impl.h @@ -0,0 +1,28 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__EVENT_IMPL_H_ +#define RCL__EVENT_IMPL_H_ + +#include "rmw/rmw.h" + +#include "rcl/event.h" + +struct rcl_event_impl_s +{ + rmw_event_t rmw_handle; + rcl_allocator_t allocator; +}; + +#endif // RCL__EVENT_IMPL_H_ diff --git a/src/rcl/expand_topic_name.c b/src/rcl/expand_topic_name.c new file mode 100644 index 0000000..313949c --- /dev/null +++ b/src/rcl/expand_topic_name.c @@ -0,0 +1,233 @@ +// 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/expand_topic_name.h" + +#include +#include + +#include "./common.h" +#include "rcl/error_handling.h" +#include "rcl/types.h" +#include "rcl/validate_topic_name.h" +#include "rcutils/error_handling.h" +#include "rcutils/format_string.h" +#include "rcutils/repl_str.h" +#include "rcutils/strdup.h" +#include "rmw/error_handling.h" +#include "rmw/types.h" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + +// built-in substitution strings +#define SUBSTITUION_NODE_NAME "{node}" +#define SUBSTITUION_NAMESPACE "{ns}" +#define SUBSTITUION_NAMESPACE2 "{namespace}" + +rcl_ret_t +rcl_expand_topic_name( + const char * input_topic_name, + const char * node_name, + const char * node_namespace, + const rcutils_string_map_t * substitutions, + rcl_allocator_t allocator, + char ** output_topic_name) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_TOPIC_NAME_INVALID); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID_NAME); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID_NAMESPACE); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_UNKNOWN_SUBSTITUTION); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + + // check arguments that could be null + RCL_CHECK_ARGUMENT_FOR_NULL(input_topic_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_namespace, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(substitutions, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(output_topic_name, RCL_RET_INVALID_ARGUMENT); + // validate the input topic + int validation_result; + rcl_ret_t ret = rcl_validate_topic_name(input_topic_name, &validation_result, NULL); + if (ret != RCL_RET_OK) { + // error message already set + return ret; + } + if (validation_result != RCL_TOPIC_NAME_VALID) { + RCL_SET_ERROR_MSG("topic name is invalid"); + return RCL_RET_TOPIC_NAME_INVALID; + } + // validate the node name + rmw_ret_t rmw_ret; + rmw_ret = rmw_validate_node_name(node_name, &validation_result, NULL); + if (rmw_ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + if (validation_result != RMW_NODE_NAME_VALID) { + RCL_SET_ERROR_MSG("node name is invalid"); + return RCL_RET_NODE_INVALID_NAME; + } + // validate the namespace + rmw_ret = rmw_validate_namespace(node_namespace, &validation_result, NULL); + if (rmw_ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + if (validation_result != RMW_NODE_NAME_VALID) { + RCL_SET_ERROR_MSG("node namespace is invalid"); + return RCL_RET_NODE_INVALID_NAMESPACE; + } + // check if the topic has substitutions to be made + bool has_a_substitution = strchr(input_topic_name, '{') != NULL; + bool has_a_namespace_tilde = input_topic_name[0] == '~'; + bool is_absolute = input_topic_name[0] == '/'; + // if absolute and doesn't have any substitution + if (is_absolute && !has_a_substitution) { + // nothing to do, duplicate and return + *output_topic_name = rcutils_strdup(input_topic_name, allocator); + if (!*output_topic_name) { + *output_topic_name = NULL; + RCL_SET_ERROR_MSG("failed to allocate memory for output topic"); + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_OK; + } + char * local_output = NULL; + // if has_a_namespace_tilde, replace that first + if (has_a_namespace_tilde) { + // special case where node_namespace is just '/' + // then no additional separating '/' is needed + const char * fmt = (strlen(node_namespace) == 1) ? "%s%s%s" : "%s/%s%s"; + local_output = + rcutils_format_string(allocator, fmt, node_namespace, node_name, input_topic_name + 1); + if (!local_output) { + *output_topic_name = NULL; + RCL_SET_ERROR_MSG("failed to allocate memory for output topic"); + return RCL_RET_BAD_ALLOC; + } + } + // if it has any substitutions, replace those + if (has_a_substitution) { + // Assumptions entering this scope about the topic string: + // + // - All {} are matched and balanced + // - There is no nesting, i.e. {{}} + // - There are no empty substitution substr, i.e. '{}' versus '{something}' + // + // These assumptions are taken because this is checked in the validation function. + const char * current_output = (local_output) ? local_output : input_topic_name; + char * next_opening_brace = NULL; + // current_output may be replaced on each loop if a substitution is made + while ((next_opening_brace = strchr(current_output, '{')) != NULL) { + char * next_closing_brace = strchr(current_output, '}'); + // conclusion based on above assumptions: next_closing_brace - next_opening_brace > 1 + size_t substitution_substr_len = next_closing_brace - next_opening_brace + 1; + // figure out what the replacement is for this substitution + const char * replacement = NULL; + if (strncmp(SUBSTITUION_NODE_NAME, next_opening_brace, substitution_substr_len) == 0) { + replacement = node_name; + } else if ( // NOLINT + strncmp(SUBSTITUION_NAMESPACE, next_opening_brace, substitution_substr_len) == 0 || + strncmp(SUBSTITUION_NAMESPACE2, next_opening_brace, substitution_substr_len) == 0) + { + replacement = node_namespace; + } else { + replacement = rcutils_string_map_getn( + substitutions, + // compare {substitution} + // ^ until ^ + next_opening_brace + 1, substitution_substr_len - 2); + if (!replacement) { + // in this case, it is neither node name nor ns nor in the substitutions map, so error + *output_topic_name = NULL; + char * unmatched_substitution = + rcutils_strndup(next_opening_brace, substitution_substr_len, allocator); + if (unmatched_substitution) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "unknown substitution: %s", unmatched_substitution); + } else { + RCUTILS_SAFE_FWRITE_TO_STDERR("failed to allocate memory for unmatched substitution\n"); + } + allocator.deallocate(unmatched_substitution, allocator.state); + allocator.deallocate(local_output, allocator.state); + return RCL_RET_UNKNOWN_SUBSTITUTION; + } + } + // at this point replacement will be set or an error would have returned out + // do the replacement + char * next_substitution = + rcutils_strndup(next_opening_brace, substitution_substr_len, allocator); + if (!next_substitution) { + *output_topic_name = NULL; + RCL_SET_ERROR_MSG("failed to allocate memory for substitution"); + allocator.deallocate(local_output, allocator.state); + return RCL_RET_BAD_ALLOC; + } + char * original_local_output = local_output; + local_output = rcutils_repl_str(current_output, next_substitution, replacement, &allocator); + allocator.deallocate(next_substitution, allocator.state); // free no matter what + allocator.deallocate(original_local_output, allocator.state); // free no matter what + if (!local_output) { + *output_topic_name = NULL; + RCL_SET_ERROR_MSG("failed to allocate memory for expanded topic"); + return RCL_RET_BAD_ALLOC; + } + current_output = local_output; + // loop until all substitutions are replaced + } // while + } + // finally make the name absolute if it isn't already + if ( + (local_output && local_output[0] != '/') || + (!local_output && input_topic_name[0] != '/')) + { + char * original_local_output = local_output; + // special case where node_namespace is just '/' + // then no additional separating '/' is needed + const char * fmt = (strlen(node_namespace) == 1) ? "%s%s" : "%s/%s"; + local_output = rcutils_format_string( + allocator, fmt, node_namespace, (local_output) ? local_output : input_topic_name); + if (original_local_output) { + allocator.deallocate(original_local_output, allocator.state); + } + if (!local_output) { + *output_topic_name = NULL; + RCL_SET_ERROR_MSG("failed to allocate memory for output topic"); + return RCL_RET_BAD_ALLOC; + } + } + // finally store the result in the out pointer and return + *output_topic_name = local_output; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_get_default_topic_name_substitutions(rcutils_string_map_t * string_map) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(string_map, RCL_RET_INVALID_ARGUMENT); + + // right now there are no default substitutions + + return RCL_RET_OK; +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/graph.c b/src/rcl/graph.c new file mode 100644 index 0000000..5ba0a0a --- /dev/null +++ b/src/rcl/graph.c @@ -0,0 +1,744 @@ +// Copyright 2016-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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/graph.h" + +#include "rcl/error_handling.h" +#include "rcl/guard_condition.h" +#include "rcl/wait.h" +#include "rcutils/allocator.h" +#include "rcutils/error_handling.h" +#include "rcutils/macros.h" +#include "rcutils/time.h" +#include "rcutils/types.h" +#include "rmw/error_handling.h" +#include "rmw/get_node_info_and_types.h" +#include "rmw/get_service_names_and_types.h" +#include "rmw/get_topic_endpoint_info.h" +#include "rmw/get_topic_names_and_types.h" +#include "rmw/names_and_types.h" +#include "rmw/rmw.h" +#include "rmw/topic_endpoint_info_array.h" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + +#include "./common.h" + +rcl_ret_t +__validate_node_name_and_namespace( + const char * node_name, + const char * node_namespace) +{ + int validation_result = 0; + rmw_ret_t rmw_ret = rmw_validate_namespace(node_namespace, &validation_result, NULL); + + if (RMW_RET_OK != rmw_ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + if (validation_result != RMW_NAMESPACE_VALID) { + const char * msg = rmw_namespace_validation_result_string(validation_result); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("%s, result: %d", msg, validation_result); + return RCL_RET_NODE_INVALID_NAMESPACE; + } + + validation_result = 0; + rmw_ret = rmw_validate_node_name(node_name, &validation_result, NULL); + if (RMW_RET_OK != rmw_ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + if (RMW_NODE_NAME_VALID != validation_result) { + const char * msg = rmw_node_name_validation_result_string(validation_result); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("%s, result: %d", msg, validation_result); + return RCL_RET_NODE_INVALID_NAME; + } + + return RCL_RET_OK; +} + +rcl_ret_t +rcl_get_publisher_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + bool no_demangle, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * topic_names_and_types) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_namespace, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT); + + const char * valid_namespace = "/"; + if (strlen(node_namespace) > 0) { + valid_namespace = node_namespace; + } + rmw_ret_t rmw_ret = rmw_names_and_types_check_zero(topic_names_and_types); + if (RMW_RET_OK != rmw_ret) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rcl_ret_t rcl_ret = __validate_node_name_and_namespace(node_name, valid_namespace); + if (RCL_RET_OK != rcl_ret) { + return rcl_ret; + } + rcutils_allocator_t rcutils_allocator = *allocator; + rmw_ret = rmw_get_publisher_names_and_types_by_node( + rcl_node_get_rmw_handle(node), + &rcutils_allocator, + node_name, + valid_namespace, + no_demangle, + topic_names_and_types + ); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_get_subscriber_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + bool no_demangle, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * topic_names_and_types) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_namespace, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT); + + const char * valid_namespace = "/"; + if (strlen(node_namespace) > 0) { + valid_namespace = node_namespace; + } + rmw_ret_t rmw_ret; + rmw_ret = rmw_names_and_types_check_zero(topic_names_and_types); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rcutils_allocator_t rcutils_allocator = *allocator; + rcl_ret_t rcl_ret = __validate_node_name_and_namespace(node_name, valid_namespace); + if (RCL_RET_OK != rcl_ret) { + return rcl_ret; + } + rmw_ret = rmw_get_subscriber_names_and_types_by_node( + rcl_node_get_rmw_handle(node), + &rcutils_allocator, + node_name, + valid_namespace, + no_demangle, + topic_names_and_types + ); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_get_service_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * service_names_and_types) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_namespace, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RCL_RET_INVALID_ARGUMENT); + + const char * valid_namespace = "/"; + if (strlen(node_namespace) > 0) { + valid_namespace = node_namespace; + } + rmw_ret_t rmw_ret; + rmw_ret = rmw_names_and_types_check_zero(service_names_and_types); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rcl_ret_t rcl_ret = __validate_node_name_and_namespace(node_name, valid_namespace); + if (RCL_RET_OK != rcl_ret) { + return rcl_ret; + } + rcutils_allocator_t rcutils_allocator = *allocator; + rmw_ret = rmw_get_service_names_and_types_by_node( + rcl_node_get_rmw_handle(node), + &rcutils_allocator, + node_name, + valid_namespace, + service_names_and_types + ); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_get_client_names_and_types_by_node( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * node_name, + const char * node_namespace, + rcl_names_and_types_t * service_names_and_types) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node_namespace, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RCL_RET_INVALID_ARGUMENT); + + const char * valid_namespace = "/"; + if (strlen(node_namespace) > 0) { + valid_namespace = node_namespace; + } + rmw_ret_t rmw_ret; + rmw_ret = rmw_names_and_types_check_zero(service_names_and_types); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rcl_ret_t rcl_ret = __validate_node_name_and_namespace(node_name, valid_namespace); + if (RCL_RET_OK != rcl_ret) { + return rcl_ret; + } + rcutils_allocator_t rcutils_allocator = *allocator; + rmw_ret = rmw_get_client_names_and_types_by_node( + rcl_node_get_rmw_handle(node), + &rcutils_allocator, + node_name, + valid_namespace, + service_names_and_types + ); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_get_topic_names_and_types( + const rcl_node_t * node, + rcl_allocator_t * allocator, + bool no_demangle, + rcl_names_and_types_t * topic_names_and_types) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t rmw_ret; + rmw_ret = rmw_names_and_types_check_zero(topic_names_and_types); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rcutils_allocator_t rcutils_allocator = *allocator; + rmw_ret = rmw_get_topic_names_and_types( + rcl_node_get_rmw_handle(node), + &rcutils_allocator, + no_demangle, + topic_names_and_types + ); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_get_service_names_and_types( + const rcl_node_t * node, + rcl_allocator_t * allocator, + rcl_names_and_types_t * service_names_and_types) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service_names_and_types, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t rmw_ret; + rmw_ret = rmw_names_and_types_check_zero(service_names_and_types); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rcutils_allocator_t rcutils_allocator = *allocator; + rmw_ret = rmw_get_service_names_and_types( + rcl_node_get_rmw_handle(node), + &rcutils_allocator, + service_names_and_types + ); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_names_and_types_init( + rcl_names_and_types_t * names_and_types, + size_t size, + rcl_allocator_t * allocator) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + + RCL_CHECK_ARGUMENT_FOR_NULL(names_and_types, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR(allocator, return RCL_RET_INVALID_ARGUMENT); + rmw_ret_t rmw_ret = rmw_names_and_types_init(names_and_types, size, allocator); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_names_and_types_fini(rcl_names_and_types_t * topic_names_and_types) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + + RCL_CHECK_ARGUMENT_FOR_NULL(topic_names_and_types, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t rmw_ret = rmw_names_and_types_fini(topic_names_and_types); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_get_node_names( + const rcl_node_t * node, + rcl_allocator_t allocator, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(node_names, RCL_RET_INVALID_ARGUMENT); + if (node_names->size != 0) { + RCL_SET_ERROR_MSG("node_names size is not zero"); + return RCL_RET_INVALID_ARGUMENT; + } + if (node_names->data) { + RCL_SET_ERROR_MSG("node_names is not null"); + return RCL_RET_INVALID_ARGUMENT; + } + RCL_CHECK_ARGUMENT_FOR_NULL(node_namespaces, RCL_RET_INVALID_ARGUMENT); + if (node_namespaces->size != 0) { + RCL_SET_ERROR_MSG("node_namespaces size is not zero"); + return RCL_RET_INVALID_ARGUMENT; + } + if (node_namespaces->data) { + RCL_SET_ERROR_MSG("node_namespaces is not null"); + return RCL_RET_INVALID_ARGUMENT; + } + (void)allocator; // to be used in rmw_get_node_names in the future + rmw_ret_t rmw_ret = rmw_get_node_names( + rcl_node_get_rmw_handle(node), + node_names, + node_namespaces); + + if (RMW_RET_OK != rmw_ret) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + + // Check that none of the node names are NULL or empty + for (size_t i = 0u; i < node_names->size; ++i) { + if (!node_names->data[i]) { + RCL_SET_ERROR_MSG("NULL node name returned by the RMW layer"); + return RCL_RET_NODE_INVALID_NAME; + } + if (!strcmp(node_names->data[i], "")) { + RCL_SET_ERROR_MSG("empty node name returned by the RMW layer"); + return RCL_RET_NODE_INVALID_NAME; + } + } + // Check that none of the node namespaces are NULL + for (size_t i = 0u; i < node_namespaces->size; ++i) { + if (!node_namespaces->data[i]) { + RCL_SET_ERROR_MSG("NULL node namespace returned by the RMW layer"); + return RCL_RET_NODE_INVALID_NAMESPACE; + } + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_get_node_names_with_enclaves( + const rcl_node_t * node, + rcl_allocator_t allocator, + rcutils_string_array_t * node_names, + rcutils_string_array_t * node_namespaces, + rcutils_string_array_t * enclaves) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(node_names, RCL_RET_INVALID_ARGUMENT); + if (node_names->size != 0) { + RCL_SET_ERROR_MSG("node_names size is not zero"); + return RCL_RET_INVALID_ARGUMENT; + } + if (node_names->data) { + RCL_SET_ERROR_MSG("node_names is not null"); + return RCL_RET_INVALID_ARGUMENT; + } + RCL_CHECK_ARGUMENT_FOR_NULL(node_namespaces, RCL_RET_INVALID_ARGUMENT); + if (node_namespaces->size != 0) { + RCL_SET_ERROR_MSG("node_namespaces size is not zero"); + return RCL_RET_INVALID_ARGUMENT; + } + if (node_namespaces->data) { + RCL_SET_ERROR_MSG("node_namespaces is not null"); + return RCL_RET_INVALID_ARGUMENT; + } + RCL_CHECK_ARGUMENT_FOR_NULL(enclaves, RCL_RET_INVALID_ARGUMENT); + if (enclaves->size != 0) { + RCL_SET_ERROR_MSG("enclaves size is not zero"); + return RCL_RET_INVALID_ARGUMENT; + } + if (enclaves->data) { + RCL_SET_ERROR_MSG("enclaves is not null"); + return RCL_RET_INVALID_ARGUMENT; + } + (void)allocator; // to be used in rmw_get_node_names in the future + rmw_ret_t rmw_ret = rmw_get_node_names_with_enclaves( + rcl_node_get_rmw_handle(node), + node_names, + node_namespaces, + enclaves); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_count_publishers( + const rcl_node_t * node, + const char * topic_name, + size_t * count) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + const rcl_node_options_t * node_options = rcl_node_get_options(node); + if (!node_options) { + return RCL_RET_NODE_INVALID; // shouldn't happen, but error is already set if so + } + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(count, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t rmw_ret = rmw_count_publishers(rcl_node_get_rmw_handle(node), topic_name, count); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_count_subscribers( + const rcl_node_t * node, + const char * topic_name, + size_t * count) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + const rcl_node_options_t * node_options = rcl_node_get_options(node); + if (!node_options) { + return RCL_RET_NODE_INVALID; // shouldn't happen, but error is already set if so + } + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(count, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t rmw_ret = rmw_count_subscribers(rcl_node_get_rmw_handle(node), topic_name, count); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +typedef rcl_ret_t (* count_entities_func_t)( + const rcl_node_t * node, + const char * topic_name, + size_t * count); + +rcl_ret_t +_rcl_wait_for_entities( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * topic_name, + const size_t expected_count, + rcutils_duration_value_t timeout, + bool * success, + count_entities_func_t count_entities_func) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(success, RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret = RCL_RET_OK; + *success = false; + + // We can avoid waiting if there are already the expected number of entities + size_t count = 0u; + ret = count_entities_func(node, topic_name, &count); + if (ret != RCL_RET_OK) { + // Error message already set + return ret; + } + if (expected_count <= count) { + *success = true; + return RCL_RET_OK; + } + + // Create a wait set and add the node graph guard condition to it + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init( + &wait_set, 0, 1, 0, 0, 0, 0, node->context, *allocator); + if (ret != RCL_RET_OK) { + // Error message already set + return ret; + } + + const rcl_guard_condition_t * guard_condition = rcl_node_get_graph_guard_condition(node); + if (!guard_condition) { + // Error message already set + ret = RCL_RET_ERROR; + goto cleanup; + } + + // Add it to the wait set + ret = rcl_wait_set_add_guard_condition(&wait_set, guard_condition, NULL); + if (ret != RCL_RET_OK) { + // Error message already set + goto cleanup; + } + + // Get current time + // We use system time to be consistent with the clock used by rcl_wait() + rcutils_time_point_value_t start; + rcutils_ret_t time_ret = rcutils_system_time_now(&start); + if (time_ret != RCUTILS_RET_OK) { + rcutils_error_string_t error = rcutils_get_error_string(); + rcutils_reset_error(); + RCL_SET_ERROR_MSG(error.str); + ret = RCL_RET_ERROR; + goto cleanup; + } + + // Wait for expected count or timeout + rcl_ret_t wait_ret; + while (true) { + // Use separate 'wait_ret' code to avoid returning spurious TIMEOUT value + wait_ret = rcl_wait(&wait_set, timeout); + if (wait_ret != RCL_RET_OK && wait_ret != RCL_RET_TIMEOUT) { + // Error message already set + ret = wait_ret; + break; + } + + // Check count + ret = count_entities_func(node, topic_name, &count); + if (ret != RCL_RET_OK) { + // Error already set + break; + } + if (expected_count <= count) { + *success = true; + break; + } + + // If we're not waiting indefinitely, compute time remaining + if (timeout >= 0) { + rcutils_time_point_value_t now; + time_ret = rcutils_system_time_now(&now); + if (time_ret != RCUTILS_RET_OK) { + rcutils_error_string_t error = rcutils_get_error_string(); + rcutils_reset_error(); + RCL_SET_ERROR_MSG(error.str); + ret = RCL_RET_ERROR; + break; + } + timeout = timeout - (now - start); + if (timeout <= 0) { + ret = RCL_RET_TIMEOUT; + break; + } + } + + // Clear wait set for next iteration + ret = rcl_wait_set_clear(&wait_set); + if (ret != RCL_RET_OK) { + // Error message already set + break; + } + } + + rcl_ret_t cleanup_ret; +cleanup: + // Cleanup + cleanup_ret = rcl_wait_set_fini(&wait_set); + if (cleanup_ret != RCL_RET_OK) { + // If we got two unexpected errors, return the earlier error + if (ret != RCL_RET_OK && ret != RCL_RET_TIMEOUT) { + // Error message already set + ret = cleanup_ret; + } + } + + return ret; +} + +rcl_ret_t +rcl_wait_for_publishers( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * topic_name, + const size_t expected_count, + rcutils_duration_value_t timeout, + bool * success) +{ + return _rcl_wait_for_entities( + node, + allocator, + topic_name, + expected_count, + timeout, + success, + rcl_count_publishers); +} + +rcl_ret_t +rcl_wait_for_subscribers( + const rcl_node_t * node, + rcl_allocator_t * allocator, + const char * topic_name, + const size_t expected_count, + rcutils_duration_value_t timeout, + bool * success) +{ + return _rcl_wait_for_entities( + node, + allocator, + topic_name, + expected_count, + timeout, + success, + rcl_count_subscribers); +} + +typedef rmw_ret_t (* get_topic_endpoint_info_func_t)( + const rmw_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * info_array); + +rcl_ret_t +__rcl_get_info_by_topic( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * info_array, + get_topic_endpoint_info_func_t get_topic_endpoint_info) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set. + } + const rcl_node_options_t * node_options = rcl_node_get_options(node); + if (!node_options) { + return RCL_RET_NODE_INVALID; // shouldn't happen, but error is already set if so + } + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + rmw_error_string_t error_string; + rmw_ret_t rmw_ret = rmw_topic_endpoint_info_array_check_zero(info_array); + if (rmw_ret != RMW_RET_OK) { + error_string = rmw_get_error_string(); + rmw_reset_error(); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "rmw_topic_endpoint_info_array_t must be zero initialized: %s,\n" + "Use rmw_get_zero_initialized_topic_endpoint_info_array", + error_string.str); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + rmw_ret = get_topic_endpoint_info( + rcl_node_get_rmw_handle(node), + allocator, + topic_name, + no_mangle, + info_array); + if (rmw_ret != RMW_RET_OK) { + error_string = rmw_get_error_string(); + rmw_reset_error(); + RCL_SET_ERROR_MSG(error_string.str); + } + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_get_publishers_info_by_topic( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * publishers_info) +{ + return __rcl_get_info_by_topic( + node, + allocator, + topic_name, + no_mangle, + publishers_info, + rmw_get_publishers_info_by_topic); +} + +rcl_ret_t +rcl_get_subscriptions_info_by_topic( + const rcl_node_t * node, + rcutils_allocator_t * allocator, + const char * topic_name, + bool no_mangle, + rmw_topic_endpoint_info_array_t * subscriptions_info) +{ + return __rcl_get_info_by_topic( + node, + allocator, + topic_name, + no_mangle, + subscriptions_info, + rmw_get_subscriptions_info_by_topic); +} + +rcl_ret_t +rcl_service_server_is_available( + const rcl_node_t * node, + const rcl_client_t * client, + bool * is_available) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + const rcl_node_options_t * node_options = rcl_node_get_options(node); + if (!node_options) { + return RCL_RET_NODE_INVALID; // shouldn't happen, but error is already set if so + } + RCL_CHECK_ARGUMENT_FOR_NULL(client, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(is_available, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t rmw_ret = rmw_service_server_is_available( + rcl_node_get_rmw_handle(node), + rcl_client_get_rmw_handle(client), + is_available + ); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/guard_condition.c b/src/rcl/guard_condition.c new file mode 100644 index 0000000..6297434 --- /dev/null +++ b/src/rcl/guard_condition.c @@ -0,0 +1,191 @@ +// Copyright 2015 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/guard_condition.h" + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "./context_impl.h" + +struct rcl_guard_condition_impl_s +{ + rmw_guard_condition_t * rmw_handle; + bool allocated_rmw_guard_condition; + rcl_guard_condition_options_t options; +}; + +rcl_guard_condition_t +rcl_get_zero_initialized_guard_condition() +{ + static rcl_guard_condition_t null_guard_condition = { + .context = 0, + .impl = 0 + }; + return null_guard_condition; +} + +rcl_ret_t +__rcl_guard_condition_init_from_rmw_impl( + rcl_guard_condition_t * guard_condition, + const rmw_guard_condition_t * rmw_guard_condition, + rcl_context_t * context, + const rcl_guard_condition_options_t options) +{ + // This function will create an rmw_guard_condition if the parameter is null. + + // Perform argument validation. + const rcl_allocator_t * allocator = &options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT); + // Ensure the guard_condition handle is zero initialized. + if (guard_condition->impl) { + RCL_SET_ERROR_MSG("guard_condition already initialized, or memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + // Make sure rcl has been initialized. + RCL_CHECK_ARGUMENT_FOR_NULL(context, RCL_RET_INVALID_ARGUMENT); + if (!rcl_context_is_valid(context)) { + RCL_SET_ERROR_MSG( + "the given context is not valid, " + "either rcl_init() was not called or rcl_shutdown() was called."); + return RCL_RET_NOT_INIT; + } + // Allocate space for the guard condition impl. + guard_condition->impl = (rcl_guard_condition_impl_t *)allocator->allocate( + sizeof(rcl_guard_condition_impl_t), allocator->state); + if (!guard_condition->impl) { + RCL_SET_ERROR_MSG("allocating memory failed"); + return RCL_RET_BAD_ALLOC; + } + // Create the rmw guard condition. + if (rmw_guard_condition) { + // If given, just assign (cast away const). + guard_condition->impl->rmw_handle = (rmw_guard_condition_t *)rmw_guard_condition; + guard_condition->impl->allocated_rmw_guard_condition = false; + } else { + // Otherwise create one. + guard_condition->impl->rmw_handle = rmw_create_guard_condition(&(context->impl->rmw_context)); + if (!guard_condition->impl->rmw_handle) { + // Deallocate impl and exit. + allocator->deallocate(guard_condition->impl, allocator->state); + guard_condition->impl = NULL; + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + guard_condition->impl->allocated_rmw_guard_condition = true; + } + // Copy options into impl. + guard_condition->impl->options = options; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_guard_condition_init( + rcl_guard_condition_t * guard_condition, + rcl_context_t * context, + const rcl_guard_condition_options_t options) +{ + // NULL indicates "create a new rmw guard condition". + return __rcl_guard_condition_init_from_rmw_impl(guard_condition, NULL, context, options); +} + +rcl_ret_t +rcl_guard_condition_init_from_rmw( + rcl_guard_condition_t * guard_condition, + const rmw_guard_condition_t * rmw_guard_condition, + rcl_context_t * context, + const rcl_guard_condition_options_t options) +{ + return __rcl_guard_condition_init_from_rmw_impl( + guard_condition, rmw_guard_condition, context, options); +} + +rcl_ret_t +rcl_guard_condition_fini(rcl_guard_condition_t * guard_condition) +{ + // Perform argument validation. + RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, RCL_RET_INVALID_ARGUMENT); + rcl_ret_t result = RCL_RET_OK; + if (guard_condition->impl) { + // assuming the allocator is valid because it is checked in rcl_guard_condition_init() + rcl_allocator_t allocator = guard_condition->impl->options.allocator; + if (guard_condition->impl->rmw_handle && guard_condition->impl->allocated_rmw_guard_condition) { + if (rmw_destroy_guard_condition(guard_condition->impl->rmw_handle) != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + result = RCL_RET_ERROR; + } + } + allocator.deallocate(guard_condition->impl, allocator.state); + guard_condition->impl = NULL; + } + return result; +} + +rcl_guard_condition_options_t +rcl_guard_condition_get_default_options() +{ + // !!! MAKE SURE THAT CHANGES TO THESE DEFAULTS ARE REFLECTED IN THE HEADER DOC STRING + static rcl_guard_condition_options_t default_options; + default_options.allocator = rcl_get_default_allocator(); + return default_options; +} + +rcl_ret_t +rcl_trigger_guard_condition(rcl_guard_condition_t * guard_condition) +{ + const rcl_guard_condition_options_t * options = rcl_guard_condition_get_options(guard_condition); + if (!options) { + return RCL_RET_INVALID_ARGUMENT; // error already set + } + // Trigger the guard condition. + if (rmw_trigger_guard_condition(guard_condition->impl->rmw_handle) != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +const rcl_guard_condition_options_t * +rcl_guard_condition_get_options(const rcl_guard_condition_t * guard_condition) +{ + // Perform argument validation. + RCL_CHECK_ARGUMENT_FOR_NULL(guard_condition, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG( + guard_condition->impl, + "guard condition implementation is invalid", + return NULL); + return &guard_condition->impl->options; +} + +rmw_guard_condition_t * +rcl_guard_condition_get_rmw_handle(const rcl_guard_condition_t * guard_condition) +{ + const rcl_guard_condition_options_t * options = rcl_guard_condition_get_options(guard_condition); + if (!options) { + return NULL; // error already set + } + return guard_condition->impl->rmw_handle; +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/init.c b/src/rcl/init.c new file mode 100644 index 0000000..92cd25d --- /dev/null +++ b/src/rcl/init.c @@ -0,0 +1,259 @@ +// Copyright 2015 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/init.h" + +#include "rcutils/logging_macros.h" +#include "rcutils/stdatomic_helper.h" +#include "rcutils/strdup.h" + +#include "rmw/error_handling.h" + +#include "tracetools/tracetools.h" + +#include "rcl/arguments.h" +#include "rcl/domain_id.h" +#include "rcl/error_handling.h" +#include "rcl/localhost.h" +#include "rcl/logging.h" +#include "rcl/security.h" +#include "rcl/validate_enclave_name.h" + +#include "./arguments_impl.h" +#include "./common.h" +#include "./context_impl.h" +#include "./init_options_impl.h" + +static atomic_uint_least64_t __rcl_next_unique_id = ATOMIC_VAR_INIT(1); + +rcl_ret_t +rcl_init( + int argc, + char const * const * argv, + const rcl_init_options_t * options, + rcl_context_t * context) +{ + rcl_ret_t fail_ret = RCL_RET_ERROR; + + if (argc > 0) { + RCL_CHECK_ARGUMENT_FOR_NULL(argv, RCL_RET_INVALID_ARGUMENT); + for (int i = 0; i < argc; ++i) { + RCL_CHECK_ARGUMENT_FOR_NULL(argv[i], RCL_RET_INVALID_ARGUMENT); + } + } else { + if (NULL != argv) { + RCL_SET_ERROR_MSG("argc is <= 0, but argv is not NULL"); + return RCL_RET_INVALID_ARGUMENT; + } + } + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options->impl, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t allocator = options->impl->allocator; + RCL_CHECK_ALLOCATOR(&allocator, return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(context, RCL_RET_INVALID_ARGUMENT); + + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, + "Initializing ROS client library, for context at address: %p", (void *) context); + + // test expectation that given context is zero initialized + if (NULL != context->impl) { + // note that this can also occur when the given context is used before initialization + // i.e. it is declared on the stack but never defined or zero initialized + RCL_SET_ERROR_MSG("rcl_init called on an already initialized context"); + return RCL_RET_ALREADY_INIT; + } + + // Zero initialize global arguments. + context->global_arguments = rcl_get_zero_initialized_arguments(); + + // Setup impl for context. + // use zero_allocate so the cleanup function will not try to clean up uninitialized parts later + context->impl = allocator.zero_allocate(1, sizeof(rcl_context_impl_t), allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + context->impl, "failed to allocate memory for context impl", return RCL_RET_BAD_ALLOC); + + // Zero initialize rmw context first so its validity can by checked in cleanup. + context->impl->rmw_context = rmw_get_zero_initialized_context(); + + // Store the allocator. + context->impl->allocator = allocator; + + // Copy the options into the context for future reference. + rcl_ret_t ret = rcl_init_options_copy(options, &(context->impl->init_options)); + if (RCL_RET_OK != ret) { + fail_ret = ret; // error message already set + goto fail; + } + + // Copy the argc and argv into the context, if argc >= 0. + context->impl->argc = argc; + context->impl->argv = NULL; + if (0 != argc && argv != NULL) { + context->impl->argv = (char **)allocator.zero_allocate(argc, sizeof(char *), allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + context->impl->argv, + "failed to allocate memory for argv", + fail_ret = RCL_RET_BAD_ALLOC; goto fail); + int64_t i; + for (i = 0; i < argc; ++i) { + size_t argv_i_length = strlen(argv[i]) + 1; + context->impl->argv[i] = (char *)allocator.allocate(argv_i_length, allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + context->impl->argv[i], + "failed to allocate memory for string entry in argv", + fail_ret = RCL_RET_BAD_ALLOC; goto fail); + memcpy(context->impl->argv[i], argv[i], argv_i_length); + } + } + + // Parse the ROS specific arguments. + ret = rcl_parse_arguments(argc, argv, allocator, &context->global_arguments); + if (RCL_RET_OK != ret) { + fail_ret = ret; + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to parse global arguments"); + goto fail; + } + + // Set the instance id. + uint64_t next_instance_id = rcutils_atomic_fetch_add_uint64_t(&__rcl_next_unique_id, 1); + if (0 == next_instance_id) { + // Roll over occurred, this is an extremely unlikely occurrence. + RCL_SET_ERROR_MSG("unique rcl instance ids exhausted"); + // Roll back to try to avoid the next call succeeding, but there's a data race here. + rcutils_atomic_store(&__rcl_next_unique_id, -1); + goto fail; + } + rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), next_instance_id); + context->impl->init_options.impl->rmw_init_options.instance_id = next_instance_id; + + size_t * domain_id = &context->impl->init_options.impl->rmw_init_options.domain_id; + if (RCL_DEFAULT_DOMAIN_ID == *domain_id) { + // Get actual domain id based on environment variable. + ret = rcl_get_default_domain_id(domain_id); + if (RCL_RET_OK != ret) { + fail_ret = ret; + goto fail; + } + } + + rmw_localhost_only_t * localhost_only = + &context->impl->init_options.impl->rmw_init_options.localhost_only; + if (RMW_LOCALHOST_ONLY_DEFAULT == *localhost_only) { + // Get actual localhost_only value based on environment variable, if needed. + ret = rcl_get_localhost_only(localhost_only); + if (RCL_RET_OK != ret) { + fail_ret = ret; + goto fail; + } + } + + if (context->global_arguments.impl->enclave) { + context->impl->init_options.impl->rmw_init_options.enclave = rcutils_strdup( + context->global_arguments.impl->enclave, + context->impl->allocator); + } else { + context->impl->init_options.impl->rmw_init_options.enclave = rcutils_strdup( + "/", context->impl->allocator); + } + + if (!context->impl->init_options.impl->rmw_init_options.enclave) { + RCL_SET_ERROR_MSG("failed to set context name"); + fail_ret = RCL_RET_BAD_ALLOC; + goto fail; + } + + int validation_result; + size_t invalid_index; + ret = rcl_validate_enclave_name( + context->impl->init_options.impl->rmw_init_options.enclave, + &validation_result, + &invalid_index); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG("rcl_validate_enclave_name() failed"); + fail_ret = ret; + goto fail; + } + if (RCL_ENCLAVE_NAME_VALID != validation_result) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Enclave name is not valid: '%s'. Invalid index: %zu", + rcl_enclave_name_validation_result_string(validation_result), + invalid_index); + fail_ret = RCL_RET_ERROR; + goto fail; + } + + rmw_security_options_t * security_options = + &context->impl->init_options.impl->rmw_init_options.security_options; + ret = rcl_get_security_options_from_environment( + context->impl->init_options.impl->rmw_init_options.enclave, + &context->impl->allocator, + security_options); + if (RCL_RET_OK != ret) { + fail_ret = ret; + goto fail; + } + + // Initialize rmw_init. + rmw_ret_t rmw_ret = rmw_init( + &(context->impl->init_options.impl->rmw_init_options), + &(context->impl->rmw_context)); + if (RMW_RET_OK != rmw_ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + fail_ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + goto fail; + } + + TRACEPOINT(rcl_init, (const void *)context); + + return RCL_RET_OK; +fail: + __cleanup_context(context); + return fail_ret; +} + +rcl_ret_t +rcl_shutdown(rcl_context_t * context) +{ + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, + "Shutting down ROS client library, for context at address: %p", (void *) context); + RCL_CHECK_ARGUMENT_FOR_NULL(context, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + context->impl, "context is zero-initialized", return RCL_RET_INVALID_ARGUMENT); + if (!rcl_context_is_valid(context)) { + RCL_SET_ERROR_MSG("rcl_shutdown already called on the given context"); + return RCL_RET_ALREADY_SHUTDOWN; + } + + rmw_ret_t rmw_ret = rmw_shutdown(&(context->impl->rmw_context)); + if (RMW_RET_OK != rmw_ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + + // reset the instance id to 0 to indicate "invalid" + rcutils_atomic_store((atomic_uint_least64_t *)(&context->instance_id_storage), 0); + + return RCL_RET_OK; +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/init_options.c b/src/rcl/init_options.c new file mode 100644 index 0000000..5fa93c7 --- /dev/null +++ b/src/rcl/init_options.c @@ -0,0 +1,178 @@ +// Copyright 2015 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/init_options.h" + +#include "./common.h" +#include "./init_options_impl.h" +#include "rcutils/macros.h" +#include "rcl/error_handling.h" +#include "rmw/error_handling.h" +#include "rcutils/logging_macros.h" + +rcl_init_options_t +rcl_get_zero_initialized_init_options(void) +{ + return (const rcl_init_options_t) { + .impl = 0, + }; // NOLINT(readability/braces): false positive +} + +/// Initialize given init_options with the default values and zero-initialize implementation. +static inline +rcl_ret_t +_rcl_init_options_zero_init(rcl_init_options_t * init_options, rcl_allocator_t allocator) +{ + init_options->impl = allocator.allocate(sizeof(rcl_init_options_impl_t), allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + init_options->impl, + "failed to allocate memory for init options impl", + return RCL_RET_BAD_ALLOC); + init_options->impl->allocator = allocator; + init_options->impl->rmw_init_options = rmw_get_zero_initialized_init_options(); + + return RCL_RET_OK; +} + +rcl_ret_t +rcl_init_options_init(rcl_init_options_t * init_options, rcl_allocator_t allocator) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ALREADY_INIT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + RCL_CHECK_ARGUMENT_FOR_NULL(init_options, RCL_RET_INVALID_ARGUMENT); + if (NULL != init_options->impl) { + RCL_SET_ERROR_MSG("given init_options (rcl_init_options_t) is already initialized"); + return RCL_RET_ALREADY_INIT; + } + RCL_CHECK_ALLOCATOR(&allocator, return RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret = _rcl_init_options_zero_init(init_options, allocator); + if (RCL_RET_OK != ret) { + return ret; + } + rmw_ret_t rmw_ret = rmw_init_options_init(&(init_options->impl->rmw_init_options), allocator); + if (RMW_RET_OK != rmw_ret) { + allocator.deallocate(init_options->impl, allocator.state); + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_init_options_copy(const rcl_init_options_t * src, rcl_init_options_t * dst) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ALREADY_INIT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + RCL_CHECK_ARGUMENT_FOR_NULL(src, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(src->impl, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR(&src->impl->allocator, return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(dst, RCL_RET_INVALID_ARGUMENT); + if (NULL != dst->impl) { + RCL_SET_ERROR_MSG("given dst (rcl_init_options_t) is already initialized"); + return RCL_RET_ALREADY_INIT; + } + + // initialize dst (since we know it's in a zero initialized state) + rcl_ret_t ret = _rcl_init_options_zero_init(dst, src->impl->allocator); + if (RCL_RET_OK != ret) { + return ret; // error already set + } + + // copy src information into dst + rmw_ret_t rmw_ret = + rmw_init_options_copy(&(src->impl->rmw_init_options), &(dst->impl->rmw_init_options)); + if (RMW_RET_OK != rmw_ret) { + rmw_error_string_t error_string = rmw_get_error_string(); + rmw_reset_error(); + ret = rcl_init_options_fini(dst); + if (RCL_RET_OK != ret) { + RCUTILS_LOG_ERROR_NAMED( + "rcl", + "failed to finalize dst rcl_init_options while handling failure to " + "copy rmw_init_options, original ret '%d' and error: %s", rmw_ret, error_string.str); + return ret; // error already set + } + RCL_SET_ERROR_MSG(error_string.str); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + + return RCL_RET_OK; +} + +rcl_ret_t +rcl_init_options_fini(rcl_init_options_t * init_options) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(init_options, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(init_options->impl, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t allocator = init_options->impl->allocator; + RCL_CHECK_ALLOCATOR(&allocator, return RCL_RET_INVALID_ARGUMENT); + rmw_ret_t rmw_ret = rmw_init_options_fini(&(init_options->impl->rmw_init_options)); + if (RMW_RET_OK != rmw_ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + allocator.deallocate(init_options->impl, allocator.state); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_init_options_get_domain_id(const rcl_init_options_t * init_options, size_t * domain_id) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(init_options, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(init_options->impl, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(domain_id, RCL_RET_INVALID_ARGUMENT); + *domain_id = init_options->impl->rmw_init_options.domain_id; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_init_options_set_domain_id(rcl_init_options_t * init_options, size_t domain_id) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(init_options, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(init_options->impl, RCL_RET_INVALID_ARGUMENT); + init_options->impl->rmw_init_options.domain_id = domain_id; + return RCL_RET_OK; +} + +rmw_init_options_t * +rcl_init_options_get_rmw_init_options(rcl_init_options_t * init_options) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(init_options, NULL); + RCL_CHECK_ARGUMENT_FOR_NULL(init_options->impl, NULL); + return &(init_options->impl->rmw_init_options); +} + +const rcl_allocator_t * +rcl_init_options_get_allocator(const rcl_init_options_t * init_options) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(init_options, NULL); + RCL_CHECK_ARGUMENT_FOR_NULL(init_options->impl, NULL); + return &(init_options->impl->allocator); +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/init_options_impl.h b/src/rcl/init_options_impl.h new file mode 100644 index 0000000..f477f8c --- /dev/null +++ b/src/rcl/init_options_impl.h @@ -0,0 +1,38 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__INIT_OPTIONS_IMPL_H_ +#define RCL__INIT_OPTIONS_IMPL_H_ + +#include "rcl/init_options.h" + +#include "rmw/init_options.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// \internal +struct rcl_init_options_impl_s +{ + rcl_allocator_t allocator; + rmw_init_options_t rmw_init_options; +}; + +#ifdef __cplusplus +} +#endif + +#endif // RCL__INIT_OPTIONS_IMPL_H_ diff --git a/src/rcl/lexer.c b/src/rcl/lexer.c new file mode 100644 index 0000000..b537548 --- /dev/null +++ b/src/rcl/lexer.c @@ -0,0 +1,675 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rcl/error_handling.h" +#include "rcl/lexer.h" + +/* The lexer tries to find a lexeme in a string. + * It looks at one character at a time, and uses that character's value to decide how to transition + * a state machine. + * A transition is taken if a character's ASCII value falls within its range. + * There is never more than one matching transition. + * + * If no transition matches then it uses a state's '' transition. + * Every state has exactly one '' transition. + * In the diagram below all states have an `` to T_NONE unless otherwise specified. + * + * When a transition is taken it causes the lexer to move to another character in the string. + * Normal transitions always move the lexer forwards one character. + * '' transitions may cause the lexer to move forwards 1, or backwards N. + * The movement M is written as M = 1 + N so it can be stored in an unsigned integer. + * For example, an `` transition with M = 0 moves the lexer forwards 1 character, M = 1 keeps + * the lexer at the current character, and M = 2 moves the lexer backwards one character. + +digraph remapping_lexer { + rankdir=LR; + node [shape = box, fontsize = 7]; + T_TILDE_SLASH + T_URL_SERVICE + T_URL_TOPIC + T_COLON + T_NODE + T_NS + T_SEPARATOR + T_BR1 + T_BR2 + T_BR3 + T_BR4 + T_BR5 + T_BR6 + T_BR7 + T_BR8 + T_BR9 + T_TOKEN + T_FORWARD_SLASH + T_WILD_ONE + T_WILD_MULTI + T_EOF + T_NONE + T_DOT + node [shape = circle]; + S0 -> T_FORWARD_SLASH [ label = "/"]; + S0 -> T_DOT [ label = "."]; + S0 -> S1 [ label = "\\"]; + S0 -> S2 [ label = "~"]; + S0 -> S3 [ label = "_" ]; + S0 -> S9 [ label = "a-qs-zA-Z"]; + S0 -> S11 [ label = "r"]; + S0 -> S30 [ label = "*"]; + S0 -> S31 [ label = ":"]; + S1 -> T_BR1 [ label = "1"]; + S1 -> T_BR2 [ label = "2"]; + S1 -> T_BR3 [ label = "3"]; + S1 -> T_BR4 [ label = "4"]; + S1 -> T_BR5 [ label = "5"]; + S1 -> T_BR6 [ label = "6"]; + S1 -> T_BR7 [ label = "7"]; + S1 -> T_BR8 [ label = "8"]; + S1 -> T_BR9 [ label = "9"]; + S2 -> T_TILDE_SLASH [ label ="/" ]; + S3 -> S4 [ label = "_" ]; + S3 -> S10 [ label = "", color = crimson, fontcolor = crimson]; + S4 -> S5 [ label = "n" ]; + S5 -> T_NS [ label = "s"]; + S5 -> S6 [ label = "o" ]; + S6 -> S8 [ label = "d" ]; + S5 -> S7 [ label = "a" ]; + S7 -> S8 [ label = "m" ]; + S8 -> T_NODE [ label = "e"]; + S9 -> T_TOKEN [ label = "", color=crimson, fontcolor=crimson]; + S9 -> S9 [ label = "a-zA-Z0-9"]; + S9 -> S10 [ label = "_"]; + S10 -> T_TOKEN [ label = "", color=crimson, fontcolor=crimson]; + S10 -> S9 [ label = "a-zA-Z0-9"]; + S11 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S11 -> S12 [ label = "o"]; + S12 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S12 -> S13 [ label = "s"]; + S13 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S13 -> S14 [ label = "t"]; + S13 -> S21 [ label = "s"]; + S14 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S14 -> S15 [ label = "o"]; + S15 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S15 -> S16 [ label = "p"]; + S16 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S16 -> S17 [ label = "i"]; + S17 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S17 -> S18 [ label = "c"]; + S18 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S18 -> S19 [ label = ":"]; + S19 -> S20 [ label = "/"]; + S19 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S20 -> T_URL_TOPIC [ label = "/"]; + S20 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S21 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S21 -> S22 [ label = "e"]; + S22 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S22 -> S23 [ label = "r"]; + S23 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S23 -> S24 [ label = "v"]; + S24 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S24 -> S25 [ label = "i"]; + S25 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S25 -> S26 [ label = "c"]; + S26 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S26 -> S27 [ label = "e"]; + S27 -> S28 [ label = ":"]; + S27 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S28 -> S29 [ label = "/"]; + S28 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S29 -> T_URL_SERVICE [ label = "/"]; + S29 -> S9 [ label = "", color=crimson, fontcolor=crimson]; + S30 -> T_WILD_MULTI[ label = "*"]; + S30 -> T_WILD_ONE [ label = "", color=crimson, fontcolor=crimson]; + S31 -> T_SEPARATOR [ label = "="]; + S31 -> T_COLON [ label = "", color=crimson, fontcolor=crimson]; +} +*/ + +/// Represents a transition from one state to another +/// \internal +typedef struct rcl_lexer_transition_s +{ + /// Index of a state to transition to + const unsigned char to_state; + /// Start of a range of chars (inclusive) which activates this transition + const char range_start; + /// End of a range of chars (inclusive) which activates this transition + const char range_end; +} rcl_lexer_transition_t; + +/// Represents a non-terminal state +/// \internal +typedef struct rcl_lexer_state_s +{ + /// Transition to this state if no other transition matches + const unsigned char else_state; + /// Movement associated with taking else state + const unsigned char else_movement; + /// Transitions in the state machine (NULL value at end of array) + const rcl_lexer_transition_t transitions[12]; +} rcl_lexer_state_t; + +#define S0 0u +#define S1 1u +#define S2 2u +#define S3 3u +#define S4 4u +#define S5 5u +#define S6 6u +#define S7 7u +#define S8 8u +#define S9 9u +#define S10 10u +#define S11 11u +#define S12 12u +#define S13 13u +#define S14 14u +#define S15 15u +#define S16 16u +#define S17 17u +#define S18 18u +#define S19 19u +#define S20 20u +#define S21 21u +#define S22 22u +#define S23 23u +#define S24 24u +#define S25 25u +#define S26 26u +#define S27 27u +#define S28 28u +#define S29 29u +#define S30 30u +#define S31 31u +#define LAST_STATE S31 + +#define T_TILDE_SLASH 32u +#define T_URL_SERVICE 33u +#define T_URL_TOPIC 34u +#define T_COLON 35u +#define T_NODE 36u +#define T_NS 37u +#define T_SEPARATOR 38u +#define T_BR1 39u +#define T_BR2 40u +#define T_BR3 41u +#define T_BR4 42u +#define T_BR5 43u +#define T_BR6 44u +#define T_BR7 45u +#define T_BR8 46u +#define T_BR9 47u +#define T_TOKEN 48u +#define T_FORWARD_SLASH 49u +#define T_WILD_ONE 50u +#define T_WILD_MULTI 51u +#define T_EOF 52u +#define T_NONE 53u +#define T_DOT 54u + +// used to figure out if a state is terminal or not +#define FIRST_TERMINAL T_TILDE_SLASH +#define LAST_TERMINAL T_NONE + +// Used to mark where the last transition is in a state +#define END_TRANSITIONS {0, '\0', '\0'} + +static const rcl_lexer_state_t g_states[LAST_STATE + 1] = +{ + // S0 + { + T_NONE, + 0u, + { + {T_FORWARD_SLASH, '/', '/'}, + {T_DOT, '.', '.'}, + {S1, '\\', '\\'}, + {S2, '~', '~'}, + {S3, '_', '_'}, + {S9, 'a', 'q'}, + {S9, 's', 'z'}, + {S9, 'A', 'Z'}, + {S11, 'r', 'r'}, + {S30, '*', '*'}, + {S31, ':', ':'}, + END_TRANSITIONS + } + }, + // S1 + { + T_NONE, + 0u, + { + {T_BR1, '1', '1'}, + {T_BR2, '2', '2'}, + {T_BR3, '3', '3'}, + {T_BR4, '4', '4'}, + {T_BR5, '5', '5'}, + {T_BR6, '6', '6'}, + {T_BR7, '7', '7'}, + {T_BR8, '8', '8'}, + {T_BR9, '9', '9'}, + END_TRANSITIONS + } + }, + // S2 + { + T_NONE, + 0u, + { + {T_TILDE_SLASH, '/', '/'}, + END_TRANSITIONS + } + }, + // S3 + { + S10, + 1u, + { + {S4, '_', '_'}, + END_TRANSITIONS + } + }, + // S4 + { + T_NONE, + 0u, + { + {S5, 'n', 'n'}, + END_TRANSITIONS + } + }, + // S5 + { + T_NONE, + 0u, + { + {T_NS, 's', 's'}, + {S6, 'o', 'o'}, + {S7, 'a', 'a'}, + END_TRANSITIONS + } + }, + // S6 + { + T_NONE, + 0u, + { + {S8, 'd', 'd'}, + END_TRANSITIONS + } + }, + // S7 + { + T_NONE, + 0u, + { + {S8, 'm', 'm'}, + END_TRANSITIONS + } + }, + // S8 + { + T_NONE, + 0u, + { + {T_NODE, 'e', 'e'}, + END_TRANSITIONS + } + }, + // S9 + { + T_TOKEN, + 1u, + { + {S9, 'a', 'z'}, + {S9, 'A', 'Z'}, + {S9, '0', '9'}, + {S10, '_', '_'}, + END_TRANSITIONS + } + }, + // S10 + { + T_TOKEN, + 1u, + { + {S9, 'a', 'z'}, + {S9, 'A', 'Z'}, + {S9, '0', '9'}, + END_TRANSITIONS + } + }, + // S11 + { + S9, + 1u, + { + {S12, 'o', 'o'}, + END_TRANSITIONS + } + }, + // S12 + { + S9, + 1u, + { + {S13, 's', 's'}, + END_TRANSITIONS + } + }, + // S13 + { + S9, + 1u, + { + {S14, 't', 't'}, + {S21, 's', 's'}, + END_TRANSITIONS + } + }, + // S14 + { + S9, + 1u, + { + {S15, 'o', 'o'}, + END_TRANSITIONS + } + }, + // S15 + { + S9, + 1u, + { + {S16, 'p', 'p'}, + END_TRANSITIONS + } + }, + // S16 + { + S9, + 1u, + { + {S17, 'i', 'i'}, + END_TRANSITIONS + } + }, + // S17 + { + S9, + 1u, + { + {S18, 'c', 'c'}, + END_TRANSITIONS + } + }, + // S18 + { + S9, + 1u, + { + {S19, ':', ':'}, + END_TRANSITIONS + } + }, + // S19 + { + S9, + 2u, + { + {S20, '/', '/'}, + END_TRANSITIONS + } + }, + // S20 + { + S9, + 3u, + { + {T_URL_TOPIC, '/', '/'}, + END_TRANSITIONS + } + }, + // S21 + { + S9, + 1u, + { + {S22, 'e', 'e'}, + END_TRANSITIONS + } + }, + // S21 + { + S9, + 1u, + { + {S23, 'r', 'r'}, + END_TRANSITIONS + } + }, + // S23 + { + S9, + 1u, + { + {S24, 'v', 'v'}, + END_TRANSITIONS + } + }, + // S24 + { + S9, + 1u, + { + {S25, 'i', 'i'}, + END_TRANSITIONS + } + }, + // S25 + { + S9, + 1u, + { + {S26, 'c', 'c'}, + END_TRANSITIONS + } + }, + // S26 + { + S9, + 1u, + { + {S27, 'e', 'e'}, + END_TRANSITIONS + } + }, + // S27 + { + S9, + 1u, + { + {S28, ':', ':'}, + END_TRANSITIONS + } + }, + // S28 + { + S9, + 2u, + { + {S29, '/', '/'}, + END_TRANSITIONS + } + }, + // S29 + { + S9, + 3u, + { + {T_URL_SERVICE, '/', '/'}, + END_TRANSITIONS + } + }, + // S30 + { + T_WILD_ONE, + 1u, + { + {T_WILD_MULTI, '*', '*'}, + END_TRANSITIONS + } + }, + // S31 + { + T_COLON, + 1u, + { + {T_SEPARATOR, '=', '='}, + END_TRANSITIONS + } + } +}; + +static const rcl_lexeme_t g_terminals[LAST_TERMINAL + 1] = { + // 0 + RCL_LEXEME_TILDE_SLASH, + // 1 + RCL_LEXEME_URL_SERVICE, + // 2 + RCL_LEXEME_URL_TOPIC, + // 3 + RCL_LEXEME_COLON, + // 4 + RCL_LEXEME_NODE, + // 5 + RCL_LEXEME_NS, + // 6 + RCL_LEXEME_SEPARATOR, + // 7 + RCL_LEXEME_BR1, + // 8 + RCL_LEXEME_BR2, + // 9 + RCL_LEXEME_BR3, + // 10 + RCL_LEXEME_BR4, + // 11 + RCL_LEXEME_BR5, + // 12 + RCL_LEXEME_BR6, + // 13 + RCL_LEXEME_BR7, + // 14 + RCL_LEXEME_BR8, + // 15 + RCL_LEXEME_BR9, + // 16 + RCL_LEXEME_TOKEN, + // 17 + RCL_LEXEME_FORWARD_SLASH, + // 18 + RCL_LEXEME_WILD_ONE, + // 19 + RCL_LEXEME_WILD_MULTI, + // 20 + RCL_LEXEME_EOF, + // 21 + RCL_LEXEME_NONE, + // 22 + RCL_LEXEME_DOT +}; + +rcl_ret_t +rcl_lexer_analyze( + const char * text, + rcl_lexeme_t * lexeme, + size_t * length) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + RCL_CHECK_ARGUMENT_FOR_NULL(text, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(lexeme, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(length, RCL_RET_INVALID_ARGUMENT); + + *length = 0u; + + if ('\0' == text[0u]) { + // Early exit if string is empty + *lexeme = RCL_LEXEME_EOF; + return RCL_RET_OK; + } + + const rcl_lexer_state_t * state; + char current_char; + size_t next_state = S0; + size_t movement; + + // Analyze one character at a time until lexeme is found + do { + if (next_state > LAST_STATE) { + // Should never happen + RCL_SET_ERROR_MSG("Internal lexer bug: next state does not exist"); + return RCL_RET_ERROR; + } + state = &(g_states[next_state]); + current_char = text[*length]; + next_state = 0u; + movement = 0u; + + // Look for a transition that contains this character in its range + size_t transition_idx = 0u; + const rcl_lexer_transition_t * transition; + do { + transition = &(state->transitions[transition_idx]); + if (transition->range_start <= current_char && transition->range_end >= current_char) { + next_state = transition->to_state; + break; + } + ++transition_idx; + } while (0u != transition->to_state); + + // if no transition was found, take the else transition + if (0u == next_state) { + next_state = state->else_state; + movement = state->else_movement; + } + + // Move the lexer to another character in the string + if (0u == movement) { + // Go forwards 1 char + ++(*length); + } else { + // Go backwards N chars + if (movement - 1u > *length) { + // Should never happen + RCL_SET_ERROR_MSG("Internal lexer bug: movement would read before start of string"); + return RCL_RET_ERROR; + } + *length -= movement - 1u; + } + } while (next_state < FIRST_TERMINAL); + + if (FIRST_TERMINAL > next_state || next_state - FIRST_TERMINAL > LAST_TERMINAL) { + // Should never happen + RCL_SET_ERROR_MSG("Internal lexer bug: terminal state does not exist"); + return RCL_RET_ERROR; + } + *lexeme = g_terminals[next_state - FIRST_TERMINAL]; + return RCL_RET_OK; +} diff --git a/src/rcl/lexer_lookahead.c b/src/rcl/lexer_lookahead.c new file mode 100644 index 0000000..c0fb0af --- /dev/null +++ b/src/rcl/lexer_lookahead.c @@ -0,0 +1,251 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rcl/error_handling.h" +#include "rcl/lexer_lookahead.h" + +struct rcl_lexer_lookahead2_impl_s +{ + // Text that is being analyzed for lexemes + const char * text; + // Where in the text analysis is being performed + size_t text_idx; + + // first character of lexeme + size_t start[2]; + // One past last character of lexeme + size_t end[2]; + // Type of lexeme + rcl_lexeme_t type[2]; + + // Allocator to use if an error occurrs + rcl_allocator_t allocator; +}; + +rcl_lexer_lookahead2_t +rcl_get_zero_initialized_lexer_lookahead2() +{ + static rcl_lexer_lookahead2_t zero_initialized = { + .impl = NULL, + }; + return zero_initialized; +} + +rcl_ret_t +rcl_lexer_lookahead2_init( + rcl_lexer_lookahead2_t * buffer, + const char * text, + rcl_allocator_t allocator) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(buffer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(text, RCL_RET_INVALID_ARGUMENT); + if (NULL != buffer->impl) { + RCL_SET_ERROR_MSG("buffer must be zero initialized"); + return RCL_RET_INVALID_ARGUMENT; + } + + buffer->impl = allocator.allocate(sizeof(rcl_lexer_lookahead2_impl_t), allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + buffer->impl, "Failed to allocate lookahead impl", return RCL_RET_BAD_ALLOC); + + buffer->impl->text = text; + buffer->impl->text_idx = 0u; + buffer->impl->start[0] = 0u; + buffer->impl->start[1] = 0u; + buffer->impl->end[0] = 0u; + buffer->impl->end[1] = 0u; + buffer->impl->type[0] = RCL_LEXEME_NONE; + buffer->impl->type[1] = RCL_LEXEME_NONE; + buffer->impl->allocator = allocator; + + return RCL_RET_OK; +} + +rcl_ret_t +rcl_lexer_lookahead2_fini( + rcl_lexer_lookahead2_t * buffer) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(buffer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + buffer->impl, "buffer finalized twice", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG( + &(buffer->impl->allocator), "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + buffer->impl->allocator.deallocate(buffer->impl, buffer->impl->allocator.state); + buffer->impl = NULL; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_lexer_lookahead2_peek( + rcl_lexer_lookahead2_t * buffer, + rcl_lexeme_t * next_type) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + + RCL_CHECK_ARGUMENT_FOR_NULL(buffer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + buffer->impl, "buffer not initialized", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(next_type, RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret; + size_t length; + + if (buffer->impl->text_idx >= buffer->impl->end[0]) { + // No buffered lexeme; get one + ret = rcl_lexer_analyze( + rcl_lexer_lookahead2_get_text(buffer), + &(buffer->impl->type[0]), + &length); + + if (RCL_RET_OK != ret) { + return ret; + } + + buffer->impl->start[0] = buffer->impl->text_idx; + buffer->impl->end[0] = buffer->impl->start[0] + length; + } + + *next_type = buffer->impl->type[0]; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_lexer_lookahead2_peek2( + rcl_lexer_lookahead2_t * buffer, + rcl_lexeme_t * next_type1, + rcl_lexeme_t * next_type2) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret; + // Peek 1 ahead first (reusing its error checking for buffer and next_type1) + ret = rcl_lexer_lookahead2_peek(buffer, next_type1); + if (RCL_RET_OK != ret) { + return ret; + } + RCL_CHECK_ARGUMENT_FOR_NULL(next_type2, RCL_RET_INVALID_ARGUMENT); + + size_t length; + + if (buffer->impl->text_idx >= buffer->impl->end[1]) { + // No buffered lexeme; get one + ret = rcl_lexer_analyze( + &(buffer->impl->text[buffer->impl->end[0]]), + &(buffer->impl->type[1]), + &length); + + if (RCL_RET_OK != ret) { + return ret; + } + + buffer->impl->start[1] = buffer->impl->end[0]; + buffer->impl->end[1] = buffer->impl->start[1] + length; + } + + *next_type2 = buffer->impl->type[1]; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_lexer_lookahead2_accept( + rcl_lexer_lookahead2_t * buffer, + const char ** lexeme_text, + size_t * lexeme_text_length) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + RCL_CHECK_ARGUMENT_FOR_NULL(buffer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG( + buffer->impl, "buffer not initialized", return RCL_RET_INVALID_ARGUMENT); + if ( + (NULL == lexeme_text && NULL != lexeme_text_length) || + (NULL != lexeme_text && NULL == lexeme_text_length)) + { + RCL_SET_ERROR_MSG("text and length must both be set or both be NULL"); + return RCL_RET_INVALID_ARGUMENT; + } + + if (RCL_LEXEME_EOF == buffer->impl->type[0]) { + // Reached EOF, nothing to accept + if (NULL != lexeme_text && NULL != lexeme_text_length) { + *lexeme_text = rcl_lexer_lookahead2_get_text(buffer); + *lexeme_text_length = 0u; + } + return RCL_RET_OK; + } + + if (buffer->impl->text_idx >= buffer->impl->end[0]) { + RCL_SET_ERROR_MSG("no lexeme to accept"); + return RCL_RET_ERROR; + } + + if (NULL != lexeme_text && NULL != lexeme_text_length) { + *lexeme_text = &(buffer->impl->text[buffer->impl->start[0]]); + *lexeme_text_length = buffer->impl->end[0] - buffer->impl->start[0]; + } + + // Advance lexer position + buffer->impl->text_idx = buffer->impl->end[0]; + + // Move second lexeme in buffer to first position + buffer->impl->start[0] = buffer->impl->start[1]; + buffer->impl->end[0] = buffer->impl->end[1]; + buffer->impl->type[0] = buffer->impl->type[1]; + + return RCL_RET_OK; +} + +rcl_ret_t +rcl_lexer_lookahead2_expect( + rcl_lexer_lookahead2_t * buffer, + rcl_lexeme_t type, + const char ** lexeme_text, + size_t * lexeme_text_length) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_WRONG_LEXEME); + + rcl_ret_t ret; + rcl_lexeme_t lexeme; + + ret = rcl_lexer_lookahead2_peek(buffer, &lexeme); + if (RCL_RET_OK != ret) { + return ret; + } + if (type != lexeme) { + if (RCL_LEXEME_NONE == lexeme || RCL_LEXEME_EOF == lexeme) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Expected lexeme type (%d) not found, search ended at index %zu", + type, buffer->impl->text_idx); + return RCL_RET_WRONG_LEXEME; + } + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Expected lexeme type %d, got %d at index %zu", type, lexeme, + buffer->impl->text_idx); + return RCL_RET_WRONG_LEXEME; + } + return rcl_lexer_lookahead2_accept(buffer, lexeme_text, lexeme_text_length); +} + +const char * +rcl_lexer_lookahead2_get_text( + const rcl_lexer_lookahead2_t * buffer) +{ + return &(buffer->impl->text[buffer->impl->text_idx]); +} diff --git a/src/rcl/localhost.c b/src/rcl/localhost.c new file mode 100644 index 0000000..74adf3e --- /dev/null +++ b/src/rcl/localhost.c @@ -0,0 +1,49 @@ +// 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 "rcl/localhost.h" + +#include +#include + +#include "rcutils/env.h" + +#include "rcl/error_handling.h" +#include "rcl/types.h" + +const char * const RCL_LOCALHOST_ENV_VAR = "ROS_LOCALHOST_ONLY"; + +rcl_ret_t +rcl_get_localhost_only(rmw_localhost_only_t * localhost_only) +{ + const char * ros_local_host_env_val = NULL; + const char * get_env_error_str = NULL; + + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCL_CHECK_ARGUMENT_FOR_NULL(localhost_only, RCL_RET_INVALID_ARGUMENT); + + get_env_error_str = rcutils_get_env(RCL_LOCALHOST_ENV_VAR, &ros_local_host_env_val); + if (NULL != get_env_error_str) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Error getting env var '" RCUTILS_STRINGIFY(RCL_LOCALHOST_ENV_VAR) "': %s\n", + get_env_error_str); + return RCL_RET_ERROR; + } + *localhost_only = (ros_local_host_env_val != NULL && + strcmp( + ros_local_host_env_val, + "1") == 0) ? RMW_LOCALHOST_ONLY_ENABLED : RMW_LOCALHOST_ONLY_DISABLED; + return RCL_RET_OK; +} diff --git a/src/rcl/log_level.c b/src/rcl/log_level.c new file mode 100644 index 0000000..acd7ec2 --- /dev/null +++ b/src/rcl/log_level.c @@ -0,0 +1,185 @@ +// 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 "rcl/log_level.h" + +#include "rcl/error_handling.h" +#include "rcutils/allocator.h" +#include "rcutils/logging_macros.h" +#include "rcutils/strdup.h" + +rcl_log_levels_t +rcl_get_zero_initialized_log_levels() +{ + const rcl_log_levels_t log_levels = { + .default_logger_level = RCUTILS_LOG_SEVERITY_UNSET, + .logger_settings = NULL, + .num_logger_settings = 0, + .capacity_logger_settings = 0, + .allocator = {NULL, NULL, NULL, NULL, NULL}, + }; + return log_levels; +} + +rcl_ret_t +rcl_log_levels_init( + rcl_log_levels_t * log_levels, const rcl_allocator_t * allocator, size_t logger_count) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + if (log_levels->logger_settings != NULL) { + RCL_SET_ERROR_MSG("invalid logger settings"); + return RCL_RET_INVALID_ARGUMENT; + } + + log_levels->default_logger_level = RCUTILS_LOG_SEVERITY_UNSET; + log_levels->logger_settings = NULL; + log_levels->num_logger_settings = 0; + log_levels->capacity_logger_settings = logger_count; + log_levels->allocator = *allocator; + + if (logger_count > 0) { + log_levels->logger_settings = allocator->allocate( + sizeof(rcl_logger_setting_t) * logger_count, allocator->state); + if (NULL == log_levels->logger_settings) { + RCL_SET_ERROR_MSG("Error allocating memory"); + return RCL_RET_BAD_ALLOC; + } + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_log_levels_copy(const rcl_log_levels_t * src, rcl_log_levels_t * dst) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(src, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(dst, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &src->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + if (dst->logger_settings != NULL) { + RCL_SET_ERROR_MSG("invalid logger settings"); + return RCL_RET_INVALID_ARGUMENT; + } + + dst->logger_settings = allocator->allocate( + sizeof(rcl_logger_setting_t) * (src->num_logger_settings), allocator->state); + if (NULL == dst->logger_settings) { + RCL_SET_ERROR_MSG("Error allocating memory"); + return RCL_RET_BAD_ALLOC; + } + + dst->default_logger_level = src->default_logger_level; + dst->capacity_logger_settings = src->capacity_logger_settings; + dst->allocator = src->allocator; + for (size_t i = 0; i < src->num_logger_settings; ++i) { + dst->logger_settings[i].name = + rcutils_strdup(src->logger_settings[i].name, *allocator); + if (NULL == dst->logger_settings[i].name) { + dst->num_logger_settings = i; + if (RCL_RET_OK != rcl_log_levels_fini(dst)) { + RCL_SET_ERROR_MSG("Error while finalizing log levels due to another error"); + } + return RCL_RET_BAD_ALLOC; + } + dst->logger_settings[i].level = src->logger_settings[i].level; + } + dst->num_logger_settings = src->num_logger_settings; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_log_levels_fini(rcl_log_levels_t * log_levels) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &log_levels->allocator; + if (log_levels->logger_settings) { + // check allocator here, so it's safe to finish a zero initialized rcl_log_levels_t + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + for (size_t i = 0; i < log_levels->num_logger_settings; ++i) { + allocator->deallocate((void *)log_levels->logger_settings[i].name, allocator->state); + } + log_levels->num_logger_settings = 0; + + allocator->deallocate(log_levels->logger_settings, allocator->state); + log_levels->logger_settings = NULL; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_log_levels_shrink_to_size(rcl_log_levels_t * log_levels) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t * allocator = &log_levels->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + if (0U == log_levels->num_logger_settings) { + allocator->deallocate(log_levels->logger_settings, allocator->state); + log_levels->logger_settings = NULL; + log_levels->capacity_logger_settings = 0; + } else if (log_levels->num_logger_settings < log_levels->capacity_logger_settings) { + rcl_logger_setting_t * new_logger_settings = allocator->reallocate( + log_levels->logger_settings, + sizeof(rcl_logger_setting_t) * log_levels->num_logger_settings, + allocator->state); + if (NULL == new_logger_settings) { + return RCL_RET_BAD_ALLOC; + } + log_levels->logger_settings = new_logger_settings; + log_levels->capacity_logger_settings = log_levels->num_logger_settings; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_log_levels_add_logger_setting( + rcl_log_levels_t * log_levels, const char * logger_name, rcl_log_severity_t log_level) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(log_levels->logger_settings, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(logger_name, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t * allocator = &log_levels->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + // check if there exists a logger with the same name + rcl_logger_setting_t * logger_setting = NULL; + for (size_t i = 0; i < log_levels->num_logger_settings; ++i) { + if (strcmp(log_levels->logger_settings[i].name, logger_name) == 0) { + logger_setting = &log_levels->logger_settings[i]; + if (logger_setting->level != log_level) { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Minimum log level of logger [%s] will be replaced from %d to %d", + logger_name, logger_setting->level, log_level); + logger_setting->level = log_level; + } + return RCL_RET_OK; + } + } + + if (log_levels->num_logger_settings >= log_levels->capacity_logger_settings) { + RCL_SET_ERROR_MSG("No capacity to store a logger setting"); + return RCL_RET_ERROR; + } + + char * name = rcutils_strdup(logger_name, *allocator); + if (NULL == name) { + RCL_SET_ERROR_MSG("failed to copy logger name"); + return RCL_RET_BAD_ALLOC; + } + + logger_setting = &log_levels->logger_settings[log_levels->num_logger_settings]; + logger_setting->name = name; + logger_setting->level = log_level; + log_levels->num_logger_settings += 1; + return RCL_RET_OK; +} diff --git a/src/rcl/logging.c b/src/rcl/logging.c new file mode 100644 index 0000000..b6bac27 --- /dev/null +++ b/src/rcl/logging.c @@ -0,0 +1,234 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include +#include + +#include "./arguments_impl.h" +#include "rcl/allocator.h" +#include "rcl/error_handling.h" +#include "rcl/logging.h" +#include "rcl_logging_interface/rcl_logging_interface.h" +#include "rcl/logging_rosout.h" +#include "rcl/macros.h" +#include "rcutils/logging.h" +#include "rcutils/time.h" + +#define RCL_LOGGING_MAX_OUTPUT_FUNCS (4) + +static rcutils_logging_output_handler_t + g_rcl_logging_out_handlers[RCL_LOGGING_MAX_OUTPUT_FUNCS] = {0}; + +static uint8_t g_rcl_logging_num_out_handlers = 0; +static rcl_allocator_t g_logging_allocator; +static bool g_rcl_logging_stdout_enabled = false; +static bool g_rcl_logging_rosout_enabled = false; +static bool g_rcl_logging_ext_lib_enabled = false; + +/** + * An output function that sends to the external logger library. + */ +static +void +rcl_logging_ext_lib_output_handler( + const rcutils_log_location_t * location, + int severity, const char * name, rcutils_time_point_value_t timestamp, + const char * format, va_list * args); + +rcl_ret_t +rcl_logging_configure_with_output_handler( + const rcl_arguments_t * global_args, + const rcl_allocator_t * allocator, + rcl_logging_output_handler_t output_handler) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(global_args, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(output_handler, RCL_RET_INVALID_ARGUMENT); + RCUTILS_LOGGING_AUTOINIT; + g_logging_allocator = *allocator; + int default_level = -1; + rcl_log_levels_t * log_levels = &global_args->impl->log_levels; + const char * config_file = global_args->impl->external_log_config_file; + g_rcl_logging_stdout_enabled = !global_args->impl->log_stdout_disabled; + g_rcl_logging_rosout_enabled = !global_args->impl->log_rosout_disabled; + g_rcl_logging_ext_lib_enabled = !global_args->impl->log_ext_lib_disabled; + rcl_ret_t status = RCL_RET_OK; + g_rcl_logging_num_out_handlers = 0; + + if (log_levels) { + if (log_levels->default_logger_level != RCUTILS_LOG_SEVERITY_UNSET) { + default_level = (int)log_levels->default_logger_level; + rcutils_logging_set_default_logger_level(default_level); + } + + for (size_t i = 0; i < log_levels->num_logger_settings; ++i) { + rcutils_ret_t rcutils_status = rcutils_logging_set_logger_level( + log_levels->logger_settings[i].name, + (int)log_levels->logger_settings[i].level); + if (RCUTILS_RET_OK != rcutils_status) { + return RCL_RET_ERROR; + } + } + } + if (g_rcl_logging_stdout_enabled) { + g_rcl_logging_out_handlers[g_rcl_logging_num_out_handlers++] = + rcutils_logging_console_output_handler; + } + if (g_rcl_logging_rosout_enabled) { + status = rcl_logging_rosout_init(allocator); + if (RCL_RET_OK == status) { + g_rcl_logging_out_handlers[g_rcl_logging_num_out_handlers++] = + rcl_logging_rosout_output_handler; + } + } + if (g_rcl_logging_ext_lib_enabled) { + status = rcl_logging_external_initialize(config_file, g_logging_allocator); + if (RCL_RET_OK == status) { + rcl_logging_ret_t logging_status = rcl_logging_external_set_logger_level( + NULL, default_level); + if (RCL_LOGGING_RET_OK != logging_status) { + status = RCL_RET_ERROR; + } + g_rcl_logging_out_handlers[g_rcl_logging_num_out_handlers++] = + rcl_logging_ext_lib_output_handler; + } + } + rcutils_logging_set_output_handler(output_handler); + return status; +} + +rcl_ret_t +rcl_logging_configure(const rcl_arguments_t * global_args, const rcl_allocator_t * allocator) +{ + return rcl_logging_configure_with_output_handler( + global_args, allocator, &rcl_logging_multiple_output_handler); +} + +rcl_ret_t rcl_logging_fini(void) +{ + rcl_ret_t status = RCL_RET_OK; + rcutils_logging_set_output_handler(rcutils_logging_console_output_handler); + // In order to output log message to `rcutils_logging_console_output_handler` + // and `rcl_logging_ext_lib_output_handler` is not called after `rcl_logging_fini`, + // in addition to calling `rcutils_logging_set_output_handler`, + // the `g_rcl_logging_num_out_handlers` and `g_rcl_logging_out_handlers` must be updated. + g_rcl_logging_num_out_handlers = 1; + g_rcl_logging_out_handlers[0] = rcutils_logging_console_output_handler; + + if (g_rcl_logging_rosout_enabled) { + status = rcl_logging_rosout_fini(); + } + if (RCL_RET_OK == status && g_rcl_logging_ext_lib_enabled) { + status = rcl_logging_external_shutdown(); + } + + return status; +} + +bool rcl_logging_rosout_enabled(void) +{ + return g_rcl_logging_rosout_enabled; +} + +void +rcl_logging_multiple_output_handler( + const rcutils_log_location_t * location, + int severity, const char * name, rcutils_time_point_value_t timestamp, + const char * format, va_list * args) +{ + for (uint8_t i = 0; + i < g_rcl_logging_num_out_handlers && NULL != g_rcl_logging_out_handlers[i]; ++i) + { + g_rcl_logging_out_handlers[i](location, severity, name, timestamp, format, args); + } +} + +static +void +rcl_logging_ext_lib_output_handler( + const rcutils_log_location_t * location, + int severity, + const char * name, + rcutils_time_point_value_t timestamp, + const char * format, + va_list * args) +{ + rcl_ret_t status; + char msg_buf[1024] = ""; + rcutils_char_array_t msg_array = { + .buffer = msg_buf, + .owns_buffer = false, + .buffer_length = 0u, + .buffer_capacity = sizeof(msg_buf), + .allocator = g_logging_allocator + }; + + char output_buf[1024] = ""; + rcutils_char_array_t output_array = { + .buffer = output_buf, + .owns_buffer = false, + .buffer_length = 0u, + .buffer_capacity = sizeof(output_buf), + .allocator = g_logging_allocator + }; + + va_list args_clone; + // The args are initialized, but clang-tidy cannot tell. + // It may be related to this bug: https://bugs.llvm.org/show_bug.cgi?id=41311 + va_copy(args_clone, *args); // NOLINT(clang-analyzer-valist.Uninitialized) + status = rcutils_char_array_vsprintf(&msg_array, format, args_clone); + va_end(args_clone); + + if (RCL_RET_OK == status) { + status = rcutils_logging_format_message( + location, severity, name, timestamp, msg_array.buffer, &output_array); + if (RCL_RET_OK != status) { + RCUTILS_SAFE_FWRITE_TO_STDERR("failed to format log message: "); + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + rcl_reset_error(); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } + } else { + RCUTILS_SAFE_FWRITE_TO_STDERR("failed to format user log message: "); + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + rcl_reset_error(); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } + rcl_logging_external_log(severity, name, output_array.buffer); + status = rcutils_char_array_fini(&msg_array); + if (RCL_RET_OK != status) { + RCUTILS_SAFE_FWRITE_TO_STDERR("failed to finalize char array: "); + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + rcl_reset_error(); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } + status = rcutils_char_array_fini(&output_array); + if (RCL_RET_OK != status) { + RCUTILS_SAFE_FWRITE_TO_STDERR("failed to finalize char array: "); + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + rcl_reset_error(); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/logging_rosout.c b/src/rcl/logging_rosout.c new file mode 100644 index 0000000..c1905d3 --- /dev/null +++ b/src/rcl/logging_rosout.c @@ -0,0 +1,303 @@ +// Copyright 2018-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 "rcl/allocator.h" +#include "rcl/error_handling.h" +#include "rcl/logging_rosout.h" +#include "rcl/node.h" +#include "rcl/publisher.h" +#include "rcl/time.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" +#include "rcl_interfaces/msg/log.h" +#include "rcutils/allocator.h" +#include "rcutils/logging_macros.h" +#include "rcutils/macros.h" +#include "rcutils/types/hash_map.h" +#include "rcutils/types/rcutils_ret.h" +#include "rosidl_runtime_c/string_functions.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define ROSOUT_TOPIC_NAME "/rosout" + +/* Return RCL_RET_OK from this macro because we won't check throughout rcl if rosout is + * initialized or not and in the case it's not we want things to continue working. + */ +#define RCL_LOGGING_ROSOUT_VERIFY_INITIALIZED \ + if (!__is_initialized) { \ + return RCL_RET_OK; \ + } + +#define RCL_RET_FROM_RCUTIL_RET(rcl_ret_var, rcutils_expr) \ + { \ + rcutils_ret_t rcutils_ret = rcutils_expr; \ + if (RCUTILS_RET_OK != rcutils_ret) { \ + if (rcutils_error_is_set()) { \ + RCL_SET_ERROR_MSG(rcutils_get_error_string().str); \ + } else { \ + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("rcutils_ret_t code: %i", rcutils_ret); \ + } \ + } \ + switch (rcutils_ret) { \ + case RCUTILS_RET_OK: \ + rcl_ret_var = RCL_RET_OK; \ + break; \ + case RCUTILS_RET_ERROR: \ + rcl_ret_var = RCL_RET_ERROR; \ + break; \ + case RCUTILS_RET_BAD_ALLOC: \ + rcl_ret_var = RCL_RET_BAD_ALLOC; \ + break; \ + case RCUTILS_RET_INVALID_ARGUMENT: \ + rcl_ret_var = RCL_RET_INVALID_ARGUMENT; \ + break; \ + case RCUTILS_RET_NOT_INITIALIZED: \ + rcl_ret_var = RCL_RET_NOT_INIT; \ + break; \ + default: \ + rcl_ret_var = RCUTILS_RET_ERROR; \ + } \ + } + +typedef struct rosout_map_entry_t +{ + rcl_node_t * node; + rcl_publisher_t publisher; +} rosout_map_entry_t; + +static rcutils_hash_map_t __logger_map; +static bool __is_initialized = false; +static rcl_allocator_t __rosout_allocator; + +rcl_ret_t rcl_logging_rosout_init( + const rcl_allocator_t * allocator) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + rcl_ret_t status = RCL_RET_OK; + if (__is_initialized) { + return RCL_RET_OK; + } + __logger_map = rcutils_get_zero_initialized_hash_map(); + RCL_RET_FROM_RCUTIL_RET( + status, + rcutils_hash_map_init( + &__logger_map, 2, sizeof(const char *), sizeof(rosout_map_entry_t), + rcutils_hash_map_string_hash_func, rcutils_hash_map_string_cmp_func, allocator)); + if (RCL_RET_OK == status) { + __rosout_allocator = *allocator; + __is_initialized = true; + } + return status; +} + +rcl_ret_t rcl_logging_rosout_fini() +{ + RCL_LOGGING_ROSOUT_VERIFY_INITIALIZED + rcl_ret_t status = RCL_RET_OK; + char * key = NULL; + rosout_map_entry_t entry; + + // fini all the outstanding publishers + rcutils_ret_t hashmap_ret = rcutils_hash_map_get_next_key_and_data( + &__logger_map, NULL, &key, &entry); + while (RCL_RET_OK == status && RCUTILS_RET_OK == hashmap_ret) { + // Teardown publisher + status = rcl_publisher_fini(&entry.publisher, entry.node); + + if (RCL_RET_OK == status) { + RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_unset(&__logger_map, &key)); + } + + if (RCL_RET_OK == status) { + hashmap_ret = rcutils_hash_map_get_next_key_and_data(&__logger_map, NULL, &key, &entry); + } + } + if (RCUTILS_RET_HASH_MAP_NO_MORE_ENTRIES != hashmap_ret) { + RCL_RET_FROM_RCUTIL_RET(status, hashmap_ret); + } + + if (RCL_RET_OK == status) { + RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_fini(&__logger_map)); + } + + if (RCL_RET_OK == status) { + __is_initialized = false; + } + return status; +} + +rcl_ret_t rcl_logging_rosout_init_publisher_for_node( + rcl_node_t * node) +{ + RCL_LOGGING_ROSOUT_VERIFY_INITIALIZED + const char * logger_name = NULL; + rosout_map_entry_t new_entry; + rcl_ret_t status = RCL_RET_OK; + + // Verify input and make sure it's not already initialized + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_NODE_INVALID); + logger_name = rcl_node_get_logger_name(node); + if (NULL == logger_name) { + RCL_SET_ERROR_MSG("Logger name was null."); + return RCL_RET_ERROR; + } + if (rcutils_hash_map_key_exists(&__logger_map, &logger_name)) { + // @TODO(nburek) Update behavior to either enforce unique names or work with non-unique + // names based on the outcome here: https://github.com/ros2/design/issues/187 + RCUTILS_LOG_WARN_NAMED( + "rcl.logging_rosout", + "Publisher already registered for provided node name. If this is due to multiple nodes " + "with the same name then all logs for that logger name will go out over the existing " + "publisher. As soon as any node with that name is destructed it will unregister the " + "publisher, preventing any further logs for that name from being published on the rosout " + "topic."); + return RCL_RET_OK; + } + + // Create a new Log message publisher on the node + const rosidl_message_type_support_t * type_support = + rosidl_typesupport_c__get_message_type_support_handle__rcl_interfaces__msg__Log(); + rcl_publisher_options_t options = rcl_publisher_get_default_options(); + + // Late joining subscriptions get the user's setting of rosout qos options. + const rcl_node_options_t * node_options = rcl_node_get_options(node); + RCL_CHECK_FOR_NULL_WITH_MSG(node_options, "Node options was null.", return RCL_RET_ERROR); + + options.qos = node_options->rosout_qos; + new_entry.publisher = rcl_get_zero_initialized_publisher(); + status = + rcl_publisher_init(&new_entry.publisher, node, type_support, ROSOUT_TOPIC_NAME, &options); + + // Add the new publisher to the map + if (RCL_RET_OK == status) { + new_entry.node = node; + RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_set(&__logger_map, &logger_name, &new_entry)); + if (RCL_RET_OK != status) { + RCL_SET_ERROR_MSG("Failed to add publisher to map."); + // We failed to add to the map so destroy the publisher that we created + rcl_ret_t fini_status = rcl_publisher_fini(&new_entry.publisher, new_entry.node); + // ignore the return status in favor of the failure from set + RCL_UNUSED(fini_status); + } + } + + return status; +} + +rcl_ret_t rcl_logging_rosout_fini_publisher_for_node( + rcl_node_t * node) +{ + RCL_LOGGING_ROSOUT_VERIFY_INITIALIZED + rosout_map_entry_t entry; + const char * logger_name = NULL; + rcl_ret_t status = RCL_RET_OK; + + // Verify input and make sure it's initialized + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_NODE_INVALID); + logger_name = rcl_node_get_logger_name(node); + if (NULL == logger_name) { + return RCL_RET_ERROR; + } + if (!rcutils_hash_map_key_exists(&__logger_map, &logger_name)) { + return RCL_RET_OK; + } + + // fini the publisher and remove the entry from the map + RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_get(&__logger_map, &logger_name, &entry)); + if (RCL_RET_OK == status) { + status = rcl_publisher_fini(&entry.publisher, entry.node); + } + if (RCL_RET_OK == status) { + RCL_RET_FROM_RCUTIL_RET(status, rcutils_hash_map_unset(&__logger_map, &logger_name)); + } + + return status; +} + +void rcl_logging_rosout_output_handler( + const rcutils_log_location_t * location, + int severity, + const char * name, + rcutils_time_point_value_t timestamp, + const char * format, + va_list * args) +{ + rosout_map_entry_t entry; + rcl_ret_t status = RCL_RET_OK; + if (!__is_initialized) { + return; + } + rcutils_ret_t rcutils_ret = rcutils_hash_map_get(&__logger_map, &name, &entry); + if (RCUTILS_RET_OK == rcutils_ret) { + char msg_buf[1024] = ""; + rcutils_char_array_t msg_array = { + .buffer = msg_buf, + .owns_buffer = false, + .buffer_length = 0u, + .buffer_capacity = sizeof(msg_buf), + .allocator = __rosout_allocator + }; + + va_list args_clone; + // The args are initialized, but clang-tidy cannot tell. + // It may be related to this bug: https://bugs.llvm.org/show_bug.cgi?id=41311 + va_copy(args_clone, *args); // NOLINT(clang-analyzer-valist.Uninitialized) + RCL_RET_FROM_RCUTIL_RET(status, rcutils_char_array_vsprintf(&msg_array, format, args_clone)); + va_end(args_clone); + if (RCL_RET_OK != status) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to format log string: "); + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + rcl_reset_error(); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } else { + rcl_interfaces__msg__Log * log_message = rcl_interfaces__msg__Log__create(); + if (NULL != log_message) { + log_message->stamp.sec = (int32_t) RCL_NS_TO_S(timestamp); + log_message->stamp.nanosec = (timestamp % RCL_S_TO_NS(1)); + log_message->level = severity; + log_message->line = (int32_t) location->line_number; + rosidl_runtime_c__String__assign(&log_message->name, name); + rosidl_runtime_c__String__assign(&log_message->msg, msg_array.buffer); + rosidl_runtime_c__String__assign(&log_message->file, location->file_name); + rosidl_runtime_c__String__assign(&log_message->function, location->function_name); + status = rcl_publish(&entry.publisher, log_message, NULL); + if (RCL_RET_OK != status) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to publish log message to rosout: "); + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + rcl_reset_error(); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } + + rcl_interfaces__msg__Log__destroy(log_message); + } + } + + RCL_RET_FROM_RCUTIL_RET(status, rcutils_char_array_fini(&msg_array)); + if (RCL_RET_OK != status) { + RCUTILS_SAFE_FWRITE_TO_STDERR("failed to fini char_array: "); + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + rcl_reset_error(); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } + } +} + + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/network_flow_endpoints.c b/src/rcl/network_flow_endpoints.c new file mode 100644 index 0000000..7a586d9 --- /dev/null +++ b/src/rcl/network_flow_endpoints.c @@ -0,0 +1,121 @@ +// Copyright 2020 Ericsson AB +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/error_handling.h" +#include "rcl/graph.h" +#include "rcl/network_flow_endpoints.h" +#include "rcl/publisher.h" +#include "rcl/subscription.h" + +#include "rcutils/allocator.h" +#include "rcutils/macros.h" +#include "rcutils/types.h" + +#include "rmw/error_handling.h" +#include "rmw/get_network_flow_endpoints.h" +#include "rmw/network_flow_endpoint_array.h" +#include "rmw/types.h" + +#include "./common.h" + +rcl_ret_t +__validate_network_flow_endpoint_array( + rcl_network_flow_endpoint_array_t * network_flow_endpoint_array) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(network_flow_endpoint_array, RCL_RET_INVALID_ARGUMENT); + + rmw_error_string_t error_string; + rmw_ret_t rmw_ret = rmw_network_flow_endpoint_array_check_zero(network_flow_endpoint_array); + if (rmw_ret != RMW_RET_OK) { + error_string = rmw_get_error_string(); + rmw_reset_error(); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "rcl_network_flow_endpoint_array_t must be zero initialized: %s,\n" + "Use rcl_get_zero_initialized_network_flow_endpoint_array", + error_string.str); + } + + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_publisher_get_network_flow_endpoints( + const rcl_publisher_t * publisher, + rcutils_allocator_t * allocator, + rcl_network_flow_endpoint_array_t * network_flow_endpoint_array) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_INVALID_ARGUMENT; + } + + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t rcl_ret = __validate_network_flow_endpoint_array( + network_flow_endpoint_array); + if (rcl_ret != RCL_RET_OK) { + return rcl_ret; + } + + rmw_error_string_t error_string; + rmw_ret_t rmw_ret = rmw_publisher_get_network_flow_endpoints( + rcl_publisher_get_rmw_handle(publisher), + allocator, + network_flow_endpoint_array); + if (rmw_ret != RMW_RET_OK) { + error_string = rmw_get_error_string(); + rmw_reset_error(); + RCL_SET_ERROR_MSG(error_string.str); + } + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_subscription_get_network_flow_endpoints( + const rcl_subscription_t * subscription, + rcutils_allocator_t * allocator, + rcl_network_flow_endpoint_array_t * network_flow_endpoint_array) +{ + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_INVALID_ARGUMENT; + } + + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t rcl_ret = __validate_network_flow_endpoint_array( + network_flow_endpoint_array); + if (rcl_ret != RCL_RET_OK) { + return rcl_ret; + } + + rmw_error_string_t error_string; + rmw_ret_t rmw_ret = rmw_subscription_get_network_flow_endpoints( + rcl_subscription_get_rmw_handle(subscription), + allocator, + network_flow_endpoint_array); + if (rmw_ret != RMW_RET_OK) { + error_string = rmw_get_error_string(); + rmw_reset_error(); + RCL_SET_ERROR_MSG(error_string.str); + } + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/node.c b/src/rcl/node.c new file mode 100644 index 0000000..4afc40f --- /dev/null +++ b/src/rcl/node.c @@ -0,0 +1,541 @@ +// Copyright 2015 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/node.h" + +#include +#include +#include +#include + +#include "rcl/arguments.h" +#include "rcl/error_handling.h" +#include "rcl/init_options.h" +#include "rcl/localhost.h" +#include "rcl/logging.h" +#include "rcl/logging_rosout.h" +#include "rcl/rcl.h" +#include "rcl/remap.h" +#include "rcl/security.h" + +#include "rcutils/env.h" +#include "rcutils/filesystem.h" +#include "rcutils/find.h" +#include "rcutils/format_string.h" +#include "rcutils/logging_macros.h" +#include "rcutils/macros.h" +#include "rcutils/repl_str.h" +#include "rcutils/snprintf.h" +#include "rcutils/strdup.h" + +#include "rmw/error_handling.h" +#include "rmw/security_options.h" +#include "rmw/rmw.h" +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" +#include "tracetools/tracetools.h" + +#include "./context_impl.h" + +const char * const RCL_DISABLE_LOANED_MESSAGES_ENV_VAR = "ROS_DISABLE_LOANED_MESSAGES"; + +struct rcl_node_impl_s +{ + rcl_node_options_t options; + rmw_node_t * rmw_node_handle; + rcl_guard_condition_t * graph_guard_condition; + const char * logger_name; + const char * fq_name; +}; + + +/// Return the logger name associated with a node given the validated node name and namespace. +/** + * E.g. for a node named "c" in namespace "/a/b", the logger name will be + * "a.b.c", assuming logger name separator of ".". + * + * \param[in] node_name validated node name (a single token) + * \param[in] node_namespace validated, absolute namespace (starting with "/") + * \param[in] allocator the allocator to use for allocation + * \returns duplicated string or null if there is an error + */ +const char * rcl_create_node_logger_name( + const char * node_name, + const char * node_namespace, + const rcl_allocator_t * allocator) +{ + // If the namespace is the root namespace ("/"), the logger name is just the node name. + if (strlen(node_namespace) == 1) { + return rcutils_strdup(node_name, *allocator); + } + + // Convert the forward slashes in the namespace to the separator used for logger names. + // The input namespace has already been expanded and therefore will always be absolute, + // i.e. it will start with a forward slash, which we want to ignore. + const char * ns_with_separators = rcutils_repl_str( + node_namespace + 1, // Ignore the leading forward slash. + "/", RCUTILS_LOGGING_SEPARATOR_STRING, + allocator); + if (NULL == ns_with_separators) { + return NULL; + } + + // Join the namespace and node name to create the logger name. + char * node_logger_name = rcutils_format_string( + *allocator, "%s%s%s", ns_with_separators, RCUTILS_LOGGING_SEPARATOR_STRING, node_name); + allocator->deallocate((char *)ns_with_separators, allocator->state); + return node_logger_name; +} + +rcl_node_t +rcl_get_zero_initialized_node() +{ + static rcl_node_t null_node = { + .context = 0, + .impl = 0 + }; + return null_node; +} + +rcl_ret_t +rcl_node_init( + rcl_node_t * node, + const char * name, + const char * namespace_, + rcl_context_t * context, + const rcl_node_options_t * options) +{ + const rmw_guard_condition_t * rmw_graph_guard_condition = NULL; + rcl_guard_condition_options_t graph_guard_condition_options = + rcl_guard_condition_get_default_options(); + rcl_ret_t ret; + rcl_ret_t fail_ret = RCL_RET_ERROR; + char * remapped_node_name = NULL; + + // Check options and allocator first, so allocator can be used for errors. + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &options->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + RCL_CHECK_ARGUMENT_FOR_NULL(name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(namespace_, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing node '%s' in namespace '%s'", name, namespace_); + if (node->impl) { + RCL_SET_ERROR_MSG("node already initialized, or struct memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + // Make sure rcl has been initialized. + RCL_CHECK_FOR_NULL_WITH_MSG( + context, "given context in options is NULL", return RCL_RET_INVALID_ARGUMENT); + if (!rcl_context_is_valid(context)) { + RCL_SET_ERROR_MSG( + "the given context is not valid, " + "either rcl_init() was not called or rcl_shutdown() was called."); + return RCL_RET_NOT_INIT; + } + // Make sure the node name is valid before allocating memory. + int validation_result = 0; + ret = rmw_validate_node_name(name, &validation_result, NULL); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return ret; + } + if (validation_result != RMW_NODE_NAME_VALID) { + const char * msg = rmw_node_name_validation_result_string(validation_result); + RCL_SET_ERROR_MSG(msg); + return RCL_RET_NODE_INVALID_NAME; + } + + // Process the namespace. + size_t namespace_length = strlen(namespace_); + const char * local_namespace_ = namespace_; + bool should_free_local_namespace_ = false; + // If the namespace is just an empty string, replace with "/" + if (namespace_length == 0) { + // Have this special case to avoid a memory allocation when "" is passed. + local_namespace_ = "/"; + } + + // If the namespace does not start with a /, add one. + if (namespace_length > 0 && namespace_[0] != '/') { + local_namespace_ = rcutils_format_string(*allocator, "/%s", namespace_); + RCL_CHECK_FOR_NULL_WITH_MSG( + local_namespace_, + "failed to format node namespace string", + ret = RCL_RET_BAD_ALLOC; goto cleanup); + should_free_local_namespace_ = true; + } + // Make sure the node namespace is valid. + validation_result = 0; + ret = rmw_validate_namespace(local_namespace_, &validation_result, NULL); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + goto cleanup; + } + if (validation_result != RMW_NAMESPACE_VALID) { + const char * msg = rmw_namespace_validation_result_string(validation_result); + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("%s, result: %d", msg, validation_result); + + ret = RCL_RET_NODE_INVALID_NAMESPACE; + goto cleanup; + } + + // Allocate space for the implementation struct. + node->impl = (rcl_node_impl_t *)allocator->allocate(sizeof(rcl_node_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + node->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup); + node->impl->rmw_node_handle = NULL; + node->impl->graph_guard_condition = NULL; + node->impl->logger_name = NULL; + node->impl->fq_name = NULL; + node->impl->options = rcl_node_get_default_options(); + node->context = context; + // Initialize node impl. + ret = rcl_node_options_copy(options, &(node->impl->options)); + if (RCL_RET_OK != ret) { + goto fail; + } + + // Remap the node name and namespace if remap rules are given + rcl_arguments_t * global_args = NULL; + if (node->impl->options.use_global_arguments) { + global_args = &(node->context->global_arguments); + } + ret = rcl_remap_node_name( + &(node->impl->options.arguments), global_args, name, *allocator, + &remapped_node_name); + if (RCL_RET_OK != ret) { + goto fail; + } else if (NULL != remapped_node_name) { + name = remapped_node_name; + } + char * remapped_namespace = NULL; + ret = rcl_remap_node_namespace( + &(node->impl->options.arguments), global_args, name, + *allocator, &remapped_namespace); + if (RCL_RET_OK != ret) { + goto fail; + } else if (NULL != remapped_namespace) { + if (should_free_local_namespace_) { + allocator->deallocate((char *)local_namespace_, allocator->state); + } + should_free_local_namespace_ = true; + local_namespace_ = remapped_namespace; + } + + // compute fully qualfied name of the node. + if ('/' == local_namespace_[strlen(local_namespace_) - 1]) { + node->impl->fq_name = rcutils_format_string(*allocator, "%s%s", local_namespace_, name); + } else { + node->impl->fq_name = rcutils_format_string(*allocator, "%s/%s", local_namespace_, name); + } + + // node logger name + node->impl->logger_name = rcl_create_node_logger_name(name, local_namespace_, allocator); + RCL_CHECK_FOR_NULL_WITH_MSG( + node->impl->logger_name, "creating logger name failed", goto fail); + + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Using domain ID of '%zu'", context->impl->rmw_context.actual_domain_id); + + node->impl->rmw_node_handle = rmw_create_node( + &(node->context->impl->rmw_context), + name, local_namespace_); + + RCL_CHECK_FOR_NULL_WITH_MSG( + node->impl->rmw_node_handle, rmw_get_error_string().str, goto fail); + // graph guard condition + rmw_graph_guard_condition = rmw_node_get_graph_guard_condition(node->impl->rmw_node_handle); + RCL_CHECK_FOR_NULL_WITH_MSG( + rmw_graph_guard_condition, rmw_get_error_string().str, goto fail); + + node->impl->graph_guard_condition = (rcl_guard_condition_t *)allocator->allocate( + sizeof(rcl_guard_condition_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + node->impl->graph_guard_condition, + "allocating memory failed", + goto fail); + *node->impl->graph_guard_condition = rcl_get_zero_initialized_guard_condition(); + graph_guard_condition_options.allocator = *allocator; + ret = rcl_guard_condition_init_from_rmw( + node->impl->graph_guard_condition, + rmw_graph_guard_condition, + context, + graph_guard_condition_options); + if (ret != RCL_RET_OK) { + // error message already set + goto fail; + } + // The initialization for the rosout publisher requires the node to be in initialized to a point + // that it can create new topic publishers + if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout) { + ret = rcl_logging_rosout_init_publisher_for_node(node); + if (ret != RCL_RET_OK) { + // error message already set + goto fail; + } + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node initialized"); + ret = RCL_RET_OK; + TRACEPOINT( + rcl_node_init, + (const void *)node, + (const void *)rcl_node_get_rmw_handle(node), + rcl_node_get_name(node), + rcl_node_get_namespace(node)); + goto cleanup; +fail: + if (node->impl) { + if (rcl_logging_rosout_enabled() && + node->impl->options.enable_rosout && + node->impl->logger_name) + { + ret = rcl_logging_rosout_fini_publisher_for_node(node); + RCUTILS_LOG_ERROR_EXPRESSION_NAMED( + (ret != RCL_RET_OK && ret != RCL_RET_NOT_INIT), + ROS_PACKAGE_NAME, "Failed to fini publisher for node: %i", ret); + allocator->deallocate((char *)node->impl->logger_name, allocator->state); + } + if (node->impl->fq_name) { + allocator->deallocate((char *)node->impl->fq_name, allocator->state); + } + if (node->impl->rmw_node_handle) { + ret = rmw_destroy_node(node->impl->rmw_node_handle); + if (ret != RMW_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "failed to fini rmw node in error recovery: %s", rmw_get_error_string().str + ); + } + } + if (node->impl->graph_guard_condition) { + ret = rcl_guard_condition_fini(node->impl->graph_guard_condition); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "failed to fini guard condition in error recovery: %s", rcl_get_error_string().str + ); + } + allocator->deallocate(node->impl->graph_guard_condition, allocator->state); + } + if (NULL != node->impl->options.arguments.impl) { + ret = rcl_arguments_fini(&(node->impl->options.arguments)); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "failed to fini arguments in error recovery: %s", rcl_get_error_string().str + ); + } + } + allocator->deallocate(node->impl, allocator->state); + } + *node = rcl_get_zero_initialized_node(); + + ret = fail_ret; + // fall through from fail -> cleanup +cleanup: + if (should_free_local_namespace_) { + allocator->deallocate((char *)local_namespace_, allocator->state); + local_namespace_ = NULL; + } + if (NULL != remapped_node_name) { + allocator->deallocate(remapped_node_name, allocator->state); + } + return ret; +} + +rcl_ret_t +rcl_node_fini(rcl_node_t * node) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing node"); + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_NODE_INVALID); + if (!node->impl) { + // Repeat calls to fini or calling fini on a zero initialized node is ok. + return RCL_RET_OK; + } + rcl_allocator_t allocator = node->impl->options.allocator; + rcl_ret_t result = RCL_RET_OK; + rcl_ret_t rcl_ret = RCL_RET_OK; + if (rcl_logging_rosout_enabled() && node->impl->options.enable_rosout) { + rcl_ret = rcl_logging_rosout_fini_publisher_for_node(node); + if (rcl_ret != RCL_RET_OK && rcl_ret != RCL_RET_NOT_INIT) { + RCL_SET_ERROR_MSG("Unable to fini publisher for node."); + result = RCL_RET_ERROR; + } + } + rmw_ret_t rmw_ret = rmw_destroy_node(node->impl->rmw_node_handle); + if (rmw_ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + result = RCL_RET_ERROR; + } + rcl_ret = rcl_guard_condition_fini(node->impl->graph_guard_condition); + if (rcl_ret != RCL_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + result = RCL_RET_ERROR; + } + allocator.deallocate(node->impl->graph_guard_condition, allocator.state); + // assuming that allocate and deallocate are ok since they are checked in init + allocator.deallocate((char *)node->impl->logger_name, allocator.state); + allocator.deallocate((char *)node->impl->fq_name, allocator.state); + if (NULL != node->impl->options.arguments.impl) { + rcl_ret_t ret = rcl_arguments_fini(&(node->impl->options.arguments)); + if (ret != RCL_RET_OK) { + return ret; + } + } + allocator.deallocate(node->impl, allocator.state); + node->impl = NULL; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Node finalized"); + return result; +} + +bool +rcl_node_is_valid_except_context(const rcl_node_t * node) +{ + RCL_CHECK_FOR_NULL_WITH_MSG(node, "rcl node pointer is invalid", return false); + RCL_CHECK_FOR_NULL_WITH_MSG(node->impl, "rcl node implementation is invalid", return false); + RCL_CHECK_FOR_NULL_WITH_MSG( + node->impl->rmw_node_handle, "rcl node's rmw handle is invalid", return false); + return true; +} + +bool +rcl_node_is_valid(const rcl_node_t * node) +{ + bool result = rcl_node_is_valid_except_context(node); + if (!result) { + return result; + } + if (!rcl_context_is_valid(node->context)) { + RCL_SET_ERROR_MSG("rcl node's context is invalid"); + return false; + } + return true; +} + +const char * +rcl_node_get_name(const rcl_node_t * node) +{ + if (!rcl_node_is_valid_except_context(node)) { + return NULL; // error already set + } + return node->impl->rmw_node_handle->name; +} + +const char * +rcl_node_get_namespace(const rcl_node_t * node) +{ + if (!rcl_node_is_valid_except_context(node)) { + return NULL; // error already set + } + return node->impl->rmw_node_handle->namespace_; +} + +const char * +rcl_node_get_fully_qualified_name(const rcl_node_t * node) +{ + if (!rcl_node_is_valid_except_context(node)) { + return NULL; // error already set + } + return node->impl->fq_name; +} + +const rcl_node_options_t * +rcl_node_get_options(const rcl_node_t * node) +{ + if (!rcl_node_is_valid_except_context(node)) { + return NULL; // error already set + } + return &node->impl->options; +} + +rcl_ret_t +rcl_node_get_domain_id(const rcl_node_t * node, size_t * domain_id) +{ + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(domain_id, RCL_RET_INVALID_ARGUMENT); + rcl_ret_t ret = rcl_context_get_domain_id(node->context, domain_id); + if (RCL_RET_OK != ret) { + return ret; + } + return RCL_RET_OK; +} + +rmw_node_t * +rcl_node_get_rmw_handle(const rcl_node_t * node) +{ + if (!rcl_node_is_valid_except_context(node)) { + return NULL; // error already set + } + return node->impl->rmw_node_handle; +} + +uint64_t +rcl_node_get_rcl_instance_id(const rcl_node_t * node) +{ + if (!rcl_node_is_valid_except_context(node)) { + return 0; // error already set + } + return rcl_context_get_instance_id(node->context); +} + +const rcl_guard_condition_t * +rcl_node_get_graph_guard_condition(const rcl_node_t * node) +{ + if (!rcl_node_is_valid_except_context(node)) { + return NULL; // error already set + } + return node->impl->graph_guard_condition; +} + +const char * +rcl_node_get_logger_name(const rcl_node_t * node) +{ + if (!rcl_node_is_valid_except_context(node)) { + return NULL; // error already set + } + return node->impl->logger_name; +} + +rcl_ret_t +rcl_get_disable_loaned_message(bool * disable_loaned_message) +{ + const char * env_val = NULL; + const char * env_error_str = NULL; + + RCL_CHECK_ARGUMENT_FOR_NULL(disable_loaned_message, RCL_RET_INVALID_ARGUMENT); + + env_error_str = rcutils_get_env(RCL_DISABLE_LOANED_MESSAGES_ENV_VAR, &env_val); + if (NULL != env_error_str) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Error getting env var: '" RCUTILS_STRINGIFY(RCL_DISABLE_LOANED_MESSAGES_ENV_VAR) "': %s\n", + env_error_str); + return RCL_RET_ERROR; + } + + *disable_loaned_message = (strcmp(env_val, "1") == 0); + return RCL_RET_OK; +} +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/node_options.c b/src/rcl/node_options.c new file mode 100644 index 0000000..34408f1 --- /dev/null +++ b/src/rcl/node_options.c @@ -0,0 +1,91 @@ +// Copyright 2015 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcutils/macros.h" + +#include "rcl/node_options.h" + +#include "rcl/arguments.h" +#include "rcl/domain_id.h" +#include "rcl/error_handling.h" +#include "rcl/logging_rosout.h" + +rcl_node_options_t +rcl_node_get_default_options() +{ + // !!! MAKE SURE THAT CHANGES TO THESE DEFAULTS ARE REFLECTED IN THE HEADER DOC STRING + rcl_node_options_t default_options = { + .allocator = rcl_get_default_allocator(), + .use_global_arguments = true, + .arguments = rcl_get_zero_initialized_arguments(), + .enable_rosout = true, + .rosout_qos = rcl_qos_profile_rosout_default, + }; + return default_options; +} + +rcl_ret_t +rcl_node_options_copy( + const rcl_node_options_t * options, + rcl_node_options_t * options_out) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(options_out, RCL_RET_INVALID_ARGUMENT); + if (options_out == options) { + RCL_SET_ERROR_MSG("Attempted to copy options into itself"); + return RCL_RET_INVALID_ARGUMENT; + } + if (NULL != options_out->arguments.impl) { + RCL_SET_ERROR_MSG("Options out must be zero initialized"); + return RCL_RET_INVALID_ARGUMENT; + } + options_out->allocator = options->allocator; + options_out->use_global_arguments = options->use_global_arguments; + options_out->enable_rosout = options->enable_rosout; + options_out->rosout_qos = options->rosout_qos; + if (NULL != options->arguments.impl) { + return rcl_arguments_copy(&(options->arguments), &(options_out->arguments)); + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_node_options_fini( + rcl_node_options_t * options) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t allocator = options->allocator; + RCL_CHECK_ALLOCATOR(&allocator, return RCL_RET_INVALID_ARGUMENT); + + if (options->arguments.impl) { + rcl_ret_t ret = rcl_arguments_fini(&options->arguments); + if (RCL_RET_OK != ret) { + RCL_SET_ERROR_MSG("Failed to fini rcl arguments"); + return ret; + } + } + + return RCL_RET_OK; +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/node_resolve_name.c b/src/rcl/node_resolve_name.c new file mode 100644 index 0000000..9c26676 --- /dev/null +++ b/src/rcl/node_resolve_name.c @@ -0,0 +1,162 @@ +// 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 "rcl/node.h" + +#include "rcutils/error_handling.h" +#include "rcutils/logging_macros.h" +#include "rcutils/types/string_map.h" + +#include "rmw/error_handling.h" +#include "rmw/validate_full_topic_name.h" + +#include "rcl/error_handling.h" +#include "rcl/expand_topic_name.h" +#include "rcl/remap.h" + +#include "./remap_impl.h" + +static +rcl_ret_t +rcl_resolve_name( + const rcl_arguments_t * local_args, + const rcl_arguments_t * global_args, + const char * input_topic_name, + const char * node_name, + const char * node_namespace, + rcl_allocator_t allocator, + bool is_service, + bool only_expand, + char ** output_topic_name) +{ + // the other arguments are checked by rcl_expand_topic_name() and rcl_remap_name() + RCL_CHECK_ARGUMENT_FOR_NULL(output_topic_name, RCL_RET_INVALID_ARGUMENT); + // Create default topic name substitutions map + rcutils_string_map_t substitutions_map = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions_map, 0, allocator); + if (rcutils_ret != RCUTILS_RET_OK) { + rcutils_error_string_t error = rcutils_get_error_string(); + rcutils_reset_error(); + RCL_SET_ERROR_MSG(error.str); + if (RCUTILS_RET_BAD_ALLOC == rcutils_ret) { + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_ERROR; + } + char * expanded_topic_name = NULL; + char * remapped_topic_name = NULL; + rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&substitutions_map); + if (ret != RCL_RET_OK) { + if (RCL_RET_BAD_ALLOC != ret) { + ret = RCL_RET_ERROR; + } + goto cleanup; + } + // expand topic name + ret = rcl_expand_topic_name( + input_topic_name, + node_name, + node_namespace, + &substitutions_map, + allocator, + &expanded_topic_name); + if (RCL_RET_OK != ret) { + goto cleanup; + } + // remap topic name + if (!only_expand) { + ret = rcl_remap_name( + local_args, global_args, is_service ? RCL_SERVICE_REMAP : RCL_TOPIC_REMAP, + expanded_topic_name, node_name, node_namespace, &substitutions_map, allocator, + &remapped_topic_name); + if (RCL_RET_OK != ret) { + goto cleanup; + } + } + if (NULL == remapped_topic_name) { + remapped_topic_name = expanded_topic_name; + expanded_topic_name = NULL; + } + // validate the result + int validation_result; + rmw_ret_t rmw_ret = rmw_validate_full_topic_name(remapped_topic_name, &validation_result, NULL); + if (rmw_ret != RMW_RET_OK) { + const char * error = rmw_get_error_string().str; + rmw_reset_error(); + RCL_SET_ERROR_MSG(error); + ret = RCL_RET_ERROR; + goto cleanup; + } + if (validation_result != RMW_TOPIC_VALID) { + RCL_SET_ERROR_MSG(rmw_full_topic_name_validation_result_string(validation_result)); + ret = RCL_RET_TOPIC_NAME_INVALID; + goto cleanup; + } + *output_topic_name = remapped_topic_name; + remapped_topic_name = NULL; + +cleanup: + rcutils_ret = rcutils_string_map_fini(&substitutions_map); + if (rcutils_ret != RCUTILS_RET_OK) { + rcutils_error_string_t error = rcutils_get_error_string(); + rcutils_reset_error(); + if (RCL_RET_OK == ret) { + RCL_SET_ERROR_MSG(error.str); + ret = RCL_RET_ERROR; + } else { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "failed to fini string_map (%d) during error handling: %s", + rcutils_ret, + error.str); + } + } + allocator.deallocate(expanded_topic_name, allocator.state); + allocator.deallocate(remapped_topic_name, allocator.state); + if (is_service && RCL_RET_TOPIC_NAME_INVALID == ret) { + ret = RCL_RET_SERVICE_NAME_INVALID; + } + return ret; +} + +rcl_ret_t +rcl_node_resolve_name( + const rcl_node_t * node, + const char * input_topic_name, + rcl_allocator_t allocator, + bool is_service, + bool only_expand, + char ** output_topic_name) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node, RCL_RET_INVALID_ARGUMENT); + const rcl_node_options_t * node_options = rcl_node_get_options(node); + if (NULL == node_options) { + return RCL_RET_ERROR; + } + rcl_arguments_t * global_args = NULL; + if (node_options->use_global_arguments) { + global_args = &(node->context->global_arguments); + } + + return rcl_resolve_name( + &(node_options->arguments), + global_args, + input_topic_name, + rcl_node_get_name(node), + rcl_node_get_namespace(node), + allocator, + is_service, + only_expand, + output_topic_name); +} diff --git a/src/rcl/publisher.c b/src/rcl/publisher.c new file mode 100644 index 0000000..47582da --- /dev/null +++ b/src/rcl/publisher.c @@ -0,0 +1,455 @@ +// Copyright 2015 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/publisher.h" + +#include +#include + +#include "rcl/allocator.h" +#include "rcl/error_handling.h" +#include "rcl/node.h" +#include "rcutils/logging_macros.h" +#include "rcutils/macros.h" +#include "rcl/time.h" +#include "rmw/time.h" +#include "rmw/error_handling.h" +#include "tracetools/tracetools.h" + +#include "./common.h" +#include "./publisher_impl.h" + +rcl_publisher_t +rcl_get_zero_initialized_publisher() +{ + static rcl_publisher_t null_publisher = {0}; + return null_publisher; +} + +rcl_ret_t +rcl_publisher_init( + rcl_publisher_t * publisher, + const rcl_node_t * node, + const rosidl_message_type_support_t * type_support, + const char * topic_name, + const rcl_publisher_options_t * options +) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ALREADY_INIT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_TOPIC_NAME_INVALID); + + rcl_ret_t fail_ret = RCL_RET_ERROR; + + // Check options and allocator first, so allocator can be used with errors. + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t * allocator = (rcl_allocator_t *)&options->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_INVALID_ARGUMENT); + if (publisher->impl) { + RCL_SET_ERROR_MSG("publisher already initialized, or memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing publisher for topic name '%s'", topic_name); + + // Expand and remap the given topic name. + char * remapped_topic_name = NULL; + rcl_ret_t ret = rcl_node_resolve_name( + node, + topic_name, + *allocator, + false, + false, + &remapped_topic_name); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { + ret = RCL_RET_TOPIC_NAME_INVALID; + } else if (ret != RCL_RET_BAD_ALLOC) { + ret = RCL_RET_ERROR; + } + goto cleanup; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Expanded and remapped topic name '%s'", remapped_topic_name); + + // Allocate space for the implementation struct. + publisher->impl = (rcl_publisher_impl_t *)allocator->allocate( + sizeof(rcl_publisher_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup); + + // Fill out implementation struct. + // rmw handle (create rmw publisher) + // TODO(wjwwood): pass along the allocator to rmw when it supports it + publisher->impl->rmw_handle = rmw_create_publisher( + rcl_node_get_rmw_handle(node), + type_support, + remapped_topic_name, + &(options->qos), + &(options->rmw_publisher_options)); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl->rmw_handle, rmw_get_error_string().str, goto fail); + // get actual qos, and store it + rmw_ret_t rmw_ret = rmw_publisher_get_actual_qos( + publisher->impl->rmw_handle, + &publisher->impl->actual_qos); + if (RMW_RET_OK != rmw_ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + goto fail; + } + publisher->impl->actual_qos.avoid_ros_namespace_conventions = + options->qos.avoid_ros_namespace_conventions; + // options + publisher->impl->options = *options; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher initialized"); + // context + publisher->impl->context = node->context; + TRACEPOINT( + rcl_publisher_init, + (const void *)publisher, + (const void *)node, + (const void *)publisher->impl->rmw_handle, + remapped_topic_name, + options->qos.depth); + goto cleanup; +fail: + if (publisher->impl) { + if (publisher->impl->rmw_handle) { + rmw_ret_t rmw_fail_ret = rmw_destroy_publisher( + rcl_node_get_rmw_handle(node), publisher->impl->rmw_handle); + if (RMW_RET_OK != rmw_fail_ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } + } + + allocator->deallocate(publisher->impl, allocator->state); + publisher->impl = NULL; + } + + ret = fail_ret; + // Fall through to cleanup +cleanup: + allocator->deallocate(remapped_topic_name, allocator->state); + return ret; +} + +rcl_ret_t +rcl_publisher_fini(rcl_publisher_t * publisher, rcl_node_t * node) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_PUBLISHER_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + rcl_ret_t result = RCL_RET_OK; + RCL_CHECK_ARGUMENT_FOR_NULL(publisher, RCL_RET_PUBLISHER_INVALID); + if (!rcl_node_is_valid_except_context(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing publisher"); + if (publisher->impl) { + rcl_allocator_t allocator = publisher->impl->options.allocator; + rmw_node_t * rmw_node = rcl_node_get_rmw_handle(node); + if (!rmw_node) { + return RCL_RET_INVALID_ARGUMENT; + } + rmw_ret_t ret = + rmw_destroy_publisher(rmw_node, publisher->impl->rmw_handle); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + result = RCL_RET_ERROR; + } + allocator.deallocate(publisher->impl, allocator.state); + publisher->impl = NULL; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Publisher finalized"); + return result; +} + +rcl_publisher_options_t +rcl_publisher_get_default_options() +{ + // !!! MAKE SURE THAT CHANGES TO THESE DEFAULTS ARE REFLECTED IN THE HEADER DOC STRING + static rcl_publisher_options_t default_options; + // Must set the allocator and qos after because they are not a compile time constant. + default_options.qos = rmw_qos_profile_default; + default_options.allocator = rcl_get_default_allocator(); + default_options.rmw_publisher_options = rmw_get_default_publisher_options(); + return default_options; +} + +rcl_ret_t +rcl_borrow_loaned_message( + const rcl_publisher_t * publisher, + const rosidl_message_type_support_t * type_support, + void ** ros_message) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; // error already set + } + return rcl_convert_rmw_ret_to_rcl_ret( + rmw_borrow_loaned_message(publisher->impl->rmw_handle, type_support, ros_message)); +} + +rcl_ret_t +rcl_return_loaned_message_from_publisher( + const rcl_publisher_t * publisher, + void * loaned_message) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message, RCL_RET_INVALID_ARGUMENT); + return rcl_convert_rmw_ret_to_rcl_ret( + rmw_return_loaned_message_from_publisher(publisher->impl->rmw_handle, loaned_message)); +} + +rcl_ret_t +rcl_publish( + const rcl_publisher_t * publisher, + const void * ros_message, + rmw_publisher_allocation_t * allocation) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_PUBLISHER_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT); + TRACEPOINT(rcl_publish, (const void *)publisher, (const void *)ros_message); + if (rmw_publish(publisher->impl->rmw_handle, ros_message, allocation) != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_publish_serialized_message( + const rcl_publisher_t * publisher, + const rcl_serialized_message_t * serialized_message, + rmw_publisher_allocation_t * allocation) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(serialized_message, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t ret = rmw_publish_serialized_message( + publisher->impl->rmw_handle, serialized_message, allocation); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + if (ret == RMW_RET_BAD_ALLOC) { + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_publish_loaned_message( + const rcl_publisher_t * publisher, + void * ros_message, + rmw_publisher_allocation_t * allocation) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t ret = rmw_publish_loaned_message(publisher->impl->rmw_handle, ros_message, allocation); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_publisher_assert_liveliness(const rcl_publisher_t * publisher) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; // error already set + } + if (rmw_publisher_assert_liveliness(publisher->impl->rmw_handle) != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_publisher_wait_for_all_acked(const rcl_publisher_t * publisher, rcl_duration_value_t timeout) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; // error already set + } + + rmw_time_t rmw_timeout; + if (timeout > 0) { + rmw_timeout.sec = RCL_NS_TO_S(timeout); + rmw_timeout.nsec = timeout % 1000000000; + } else if (timeout < 0) { + rmw_time_t infinite = RMW_DURATION_INFINITE; + rmw_timeout = infinite; + } else { + rmw_time_t zero = RMW_DURATION_UNSPECIFIED; + rmw_timeout = zero; + } + + rmw_ret_t ret = rmw_publisher_wait_for_all_acked(publisher->impl->rmw_handle, rmw_timeout); + if (ret != RMW_RET_OK) { + if (ret == RMW_RET_TIMEOUT) { + return RCL_RET_TIMEOUT; + } + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + if (ret == RMW_RET_UNSUPPORTED) { + return RCL_RET_UNSUPPORTED; + } else { + return RCL_RET_ERROR; + } + } + + return RCL_RET_OK; +} + +const char * +rcl_publisher_get_topic_name(const rcl_publisher_t * publisher) +{ + if (!rcl_publisher_is_valid_except_context(publisher)) { + return NULL; // error already set + } + return publisher->impl->rmw_handle->topic_name; +} + +#define _publisher_get_options(pub) & pub->impl->options + +const rcl_publisher_options_t * +rcl_publisher_get_options(const rcl_publisher_t * publisher) +{ + if (!rcl_publisher_is_valid_except_context(publisher)) { + return NULL; // error already set + } + return _publisher_get_options(publisher); +} + +rmw_publisher_t * +rcl_publisher_get_rmw_handle(const rcl_publisher_t * publisher) +{ + if (!rcl_publisher_is_valid_except_context(publisher)) { + return NULL; // error already set + } + return publisher->impl->rmw_handle; +} + +rcl_context_t * +rcl_publisher_get_context(const rcl_publisher_t * publisher) +{ + if (!rcl_publisher_is_valid_except_context(publisher)) { + return NULL; // error already set + } + return publisher->impl->context; +} + +bool +rcl_publisher_is_valid(const rcl_publisher_t * publisher) +{ + if (!rcl_publisher_is_valid_except_context(publisher)) { + return false; // error already set + } + if (!rcl_context_is_valid(publisher->impl->context)) { + RCL_SET_ERROR_MSG("publisher's context is invalid"); + return false; + } + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl->rmw_handle, "publisher's rmw handle is invalid", return false); + return true; +} + +bool +rcl_publisher_is_valid_except_context(const rcl_publisher_t * publisher) +{ + RCL_CHECK_FOR_NULL_WITH_MSG(publisher, "publisher pointer is invalid", return false); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl, "publisher implementation is invalid", return false); + RCL_CHECK_FOR_NULL_WITH_MSG( + publisher->impl->rmw_handle, "publisher's rmw handle is invalid", return false); + return true; +} + +rcl_ret_t +rcl_publisher_get_subscription_count( + const rcl_publisher_t * publisher, + size_t * subscription_count) +{ + if (!rcl_publisher_is_valid(publisher)) { + return RCL_RET_PUBLISHER_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(subscription_count, RCL_RET_INVALID_ARGUMENT); + + rmw_ret_t ret = rmw_publisher_count_matched_subscriptions( + publisher->impl->rmw_handle, subscription_count); + + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + return RCL_RET_OK; +} + +const rmw_qos_profile_t * +rcl_publisher_get_actual_qos(const rcl_publisher_t * publisher) +{ + if (!rcl_publisher_is_valid_except_context(publisher)) { + return NULL; + } + return &publisher->impl->actual_qos; +} + +bool +rcl_publisher_can_loan_messages(const rcl_publisher_t * publisher) +{ + if (!rcl_publisher_is_valid(publisher)) { + return false; // error message already set + } + + bool disable_loaned_message = false; + rcl_ret_t ret = rcl_get_disable_loaned_message(&disable_loaned_message); + if (ret == RCL_RET_OK && disable_loaned_message) { + return false; + } + + return publisher->impl->rmw_handle->can_loan_messages; +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/publisher_impl.h b/src/rcl/publisher_impl.h new file mode 100644 index 0000000..cd5ff23 --- /dev/null +++ b/src/rcl/publisher_impl.h @@ -0,0 +1,30 @@ +// Copyright 2015 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 RCL__PUBLISHER_IMPL_H_ +#define RCL__PUBLISHER_IMPL_H_ + +#include "rmw/rmw.h" + +#include "rcl/publisher.h" + +struct rcl_publisher_impl_s +{ + rcl_publisher_options_t options; + rmw_qos_profile_t actual_qos; + rcl_context_t * context; + rmw_publisher_t * rmw_handle; +}; + +#endif // RCL__PUBLISHER_IMPL_H_ diff --git a/src/rcl/remap.c b/src/rcl/remap.c new file mode 100644 index 0000000..d767ddd --- /dev/null +++ b/src/rcl/remap.c @@ -0,0 +1,365 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rcl/remap.h" + +#include "./arguments_impl.h" +#include "./remap_impl.h" +#include "rcl/error_handling.h" +#include "rcl/expand_topic_name.h" +#include "rcutils/allocator.h" +#include "rcutils/macros.h" +#include "rcutils/strdup.h" +#include "rcutils/types/string_map.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +rcl_remap_t +rcl_get_zero_initialized_remap(void) +{ + static rcl_remap_t default_rule = { + .impl = NULL + }; + return default_rule; +} + +rcl_ret_t +rcl_remap_copy( + const rcl_remap_t * rule, + rcl_remap_t * rule_out) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + + RCL_CHECK_ARGUMENT_FOR_NULL(rule, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(rule_out, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(rule->impl, RCL_RET_INVALID_ARGUMENT); + + if (NULL != rule_out->impl) { + RCL_SET_ERROR_MSG("rule_out must be zero initialized"); + return RCL_RET_INVALID_ARGUMENT; + } + + rcl_allocator_t allocator = rule->impl->allocator; + + rule_out->impl = allocator.allocate(sizeof(rcl_remap_impl_t), allocator.state); + if (NULL == rule_out->impl) { + return RCL_RET_BAD_ALLOC; + } + + rule_out->impl->allocator = allocator; + + // Zero so it's safe to call rcl_remap_fini() if an error occurs while copying. + rule_out->impl->type = RCL_UNKNOWN_REMAP; + rule_out->impl->node_name = NULL; + rule_out->impl->match = NULL; + rule_out->impl->replacement = NULL; + + rule_out->impl->type = rule->impl->type; + if (NULL != rule->impl->node_name) { + rule_out->impl->node_name = rcutils_strdup(rule->impl->node_name, allocator); + if (NULL == rule_out->impl->node_name) { + goto fail; + } + } + if (NULL != rule->impl->match) { + rule_out->impl->match = rcutils_strdup(rule->impl->match, allocator); + if (NULL == rule_out->impl->match) { + goto fail; + } + } + if (NULL != rule->impl->replacement) { + rule_out->impl->replacement = rcutils_strdup( + rule->impl->replacement, allocator); + if (NULL == rule_out->impl->replacement) { + goto fail; + } + } + return RCL_RET_OK; +fail: + if (RCL_RET_OK != rcl_remap_fini(rule_out)) { + RCL_SET_ERROR_MSG("Error while finalizing remap rule due to another error"); + } + return RCL_RET_BAD_ALLOC; +} + +/// Get the first matching rule in a chain. +/// \return RCL_RET_OK if no errors occurred while searching for a rule +static +rcl_ret_t +rcl_remap_first_match( + rcl_remap_t * remap_rules, + int num_rules, + rcl_remap_type_t type_bitmask, + const char * name, + const char * node_name, + const char * node_namespace, + const rcutils_string_map_t * substitutions, + rcutils_allocator_t allocator, + rcl_remap_t ** output_rule) +{ + *output_rule = NULL; + for (int i = 0; i < num_rules; ++i) { + rcl_remap_t * rule = &(remap_rules[i]); + if (!(rule->impl->type & type_bitmask)) { + // Not the type of remap rule we're looking fore + continue; + } + if (rule->impl->node_name != NULL && 0 != strcmp(rule->impl->node_name, node_name)) { + // Rule has a node name prefix and the supplied node name didn't match + continue; + } + bool matched = false; + if (rule->impl->type & (RCL_TOPIC_REMAP | RCL_SERVICE_REMAP)) { + // topic and service rules need the match side to be expanded to a FQN + char * expanded_match = NULL; + rcl_ret_t ret = rcl_expand_topic_name( + rule->impl->match, node_name, node_namespace, + substitutions, allocator, &expanded_match); + if (RCL_RET_OK != ret) { + rcl_reset_error(); + if ( + RCL_RET_NODE_INVALID_NAMESPACE == ret || + RCL_RET_NODE_INVALID_NAME == ret || + RCL_RET_BAD_ALLOC == ret) + { + // these are probably going to happen again. Stop processing rules + return ret; + } + continue; + } + if (NULL != name) { + // this check is to satisfy clang-tidy – name is always not null when type_bitmask is + // RCL_TOPIC_REMAP or RCL_SERVICE_REMAP. That is guaranteed because rcl_remap_first_match + // and rcl_remap_name are not public. + matched = (0 == strcmp(expanded_match, name)); + } + allocator.deallocate(expanded_match, allocator.state); + } else { + // nodename and namespace replacement apply if the type and node name prefix checks passed + matched = true; + } + if (matched) { + *output_rule = rule; + break; + } + } + return RCL_RET_OK; +} + +/// Remap from one name to another using rules matching a given type bitmask. +RCL_LOCAL +rcl_ret_t +rcl_remap_name( + const rcl_arguments_t * local_arguments, + const rcl_arguments_t * global_arguments, + rcl_remap_type_t type_bitmask, + const char * name, + const char * node_name, + const char * node_namespace, + const rcutils_string_map_t * substitutions, + rcl_allocator_t allocator, + char ** output_name) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(output_name, RCL_RET_INVALID_ARGUMENT); + if (NULL != local_arguments && NULL == local_arguments->impl) { + local_arguments = NULL; + } + if (NULL != global_arguments && NULL == global_arguments->impl) { + global_arguments = NULL; + } + if (NULL == local_arguments && NULL == global_arguments) { + RCL_SET_ERROR_MSG("local_arguments invalid and not using global arguments"); + return RCL_RET_INVALID_ARGUMENT; + } + + *output_name = NULL; + rcl_remap_t * rule = NULL; + + // Look at local rules first + if (NULL != local_arguments) { + rcl_ret_t ret = rcl_remap_first_match( + local_arguments->impl->remap_rules, local_arguments->impl->num_remap_rules, type_bitmask, + name, node_name, node_namespace, substitutions, allocator, &rule); + if (ret != RCL_RET_OK) { + return ret; + } + } + // Check global rules if no local rule matched + if (NULL == rule && NULL != global_arguments) { + rcl_ret_t ret = rcl_remap_first_match( + global_arguments->impl->remap_rules, global_arguments->impl->num_remap_rules, type_bitmask, + name, node_name, node_namespace, substitutions, allocator, &rule); + if (ret != RCL_RET_OK) { + return ret; + } + } + // Do the remapping + if (NULL != rule) { + if (rule->impl->type & (RCL_TOPIC_REMAP | RCL_SERVICE_REMAP)) { + // topic and service rules need the replacement to be expanded to a FQN + rcl_ret_t ret = rcl_expand_topic_name( + rule->impl->replacement, node_name, node_namespace, substitutions, allocator, output_name); + if (RCL_RET_OK != ret) { + return ret; + } + } else { + // nodename and namespace rules don't need replacment expanded + *output_name = rcutils_strdup(rule->impl->replacement, allocator); + } + if (NULL == *output_name) { + RCL_SET_ERROR_MSG("Failed to set output"); + return RCL_RET_ERROR; + } + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_remap_topic_name( + const rcl_arguments_t * local_arguments, + const rcl_arguments_t * global_arguments, + const char * topic_name, + const char * node_name, + const char * node_namespace, + rcl_allocator_t allocator, + char ** output_name) +{ + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + + rcutils_string_map_t substitutions = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions, 0, allocator); + rcl_ret_t ret = RCL_RET_ERROR; + if (RCUTILS_RET_OK == rcutils_ret) { + ret = rcl_get_default_topic_name_substitutions(&substitutions); + if (RCL_RET_OK == ret) { + ret = rcl_remap_name( + local_arguments, global_arguments, RCL_TOPIC_REMAP, topic_name, node_name, + node_namespace, &substitutions, allocator, output_name); + } + } + if (RCUTILS_RET_OK != rcutils_string_map_fini(&substitutions)) { + return RCL_RET_ERROR; + } + return ret; +} + +rcl_ret_t +rcl_remap_service_name( + const rcl_arguments_t * local_arguments, + const rcl_arguments_t * global_arguments, + const char * service_name, + const char * node_name, + const char * node_namespace, + rcl_allocator_t allocator, + char ** output_name) +{ + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT); + + rcutils_string_map_t substitutions = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcutils_ret = rcutils_string_map_init(&substitutions, 0, allocator); + rcl_ret_t ret = RCL_RET_ERROR; + if (rcutils_ret == RCUTILS_RET_OK) { + ret = rcl_get_default_topic_name_substitutions(&substitutions); + if (ret == RCL_RET_OK) { + ret = rcl_remap_name( + local_arguments, global_arguments, RCL_SERVICE_REMAP, service_name, node_name, + node_namespace, &substitutions, allocator, output_name); + } + } + if (RCUTILS_RET_OK != rcutils_string_map_fini(&substitutions)) { + return RCL_RET_ERROR; + } + return ret; +} + +rcl_ret_t +rcl_remap_node_name( + const rcl_arguments_t * local_arguments, + const rcl_arguments_t * global_arguments, + const char * node_name, + rcl_allocator_t allocator, + char ** output_name) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID_NAME); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_INVALID_ARGUMENT); + return rcl_remap_name( + local_arguments, global_arguments, RCL_NODENAME_REMAP, NULL, node_name, NULL, NULL, + allocator, output_name); +} + +rcl_ret_t +rcl_remap_node_namespace( + const rcl_arguments_t * local_arguments, + const rcl_arguments_t * global_arguments, + const char * node_name, + rcl_allocator_t allocator, + char ** output_namespace) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID_NAMESPACE); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + RCL_CHECK_ARGUMENT_FOR_NULL(node_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "allocator is invalid", return RCL_RET_INVALID_ARGUMENT); + return rcl_remap_name( + local_arguments, global_arguments, RCL_NAMESPACE_REMAP, NULL, node_name, NULL, NULL, + allocator, output_namespace); +} + +rcl_ret_t +rcl_remap_fini( + rcl_remap_t * rule) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(rule, RCL_RET_INVALID_ARGUMENT); + if (rule->impl) { + rcl_ret_t ret = RCL_RET_OK; + if (NULL != rule->impl->node_name) { + rule->impl->allocator.deallocate( + rule->impl->node_name, rule->impl->allocator.state); + rule->impl->node_name = NULL; + } + if (NULL != rule->impl->match) { + rule->impl->allocator.deallocate( + rule->impl->match, rule->impl->allocator.state); + rule->impl->match = NULL; + } + if (NULL != rule->impl->replacement) { + rule->impl->allocator.deallocate( + rule->impl->replacement, rule->impl->allocator.state); + rule->impl->replacement = NULL; + } + rule->impl->allocator.deallocate(rule->impl, rule->impl->allocator.state); + rule->impl = NULL; + return ret; + } + RCL_SET_ERROR_MSG("rcl_remap_t finalized twice"); + return RCL_RET_ERROR; +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/remap_impl.h b/src/rcl/remap_impl.h new file mode 100644 index 0000000..3948847 --- /dev/null +++ b/src/rcl/remap_impl.h @@ -0,0 +1,71 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__REMAP_IMPL_H_ +#define RCL__REMAP_IMPL_H_ + +#include "rcl/allocator.h" +#include "rcl/macros.h" +#include "rcl/remap.h" +#include "rcl/types.h" +#include "rcl/visibility_control.h" + +#ifdef __cplusplus +extern "C" +{ +#endif + +/// Enum doubles as a bitmask for rule sthat apply to both topics and services. +typedef enum rcl_remap_type_t +{ + RCL_UNKNOWN_REMAP = 0, + RCL_TOPIC_REMAP = 1u << 0, + RCL_SERVICE_REMAP = 1u << 1, + RCL_NODENAME_REMAP = 1u << 2, + RCL_NAMESPACE_REMAP = 1u << 3 +} rcl_remap_type_t; + +struct rcl_remap_impl_s +{ + /// Bitmask indicating what type of rule this is. + rcl_remap_type_t type; + /// A node name that this rule is limited to, or NULL if it applies to any node. + char * node_name; + /// Match portion of a rule, or NULL if node name or namespace replacement. + char * match; + /// Replacement portion of a rule. + char * replacement; + + /// Allocator used to allocate objects in this struct + rcl_allocator_t allocator; +}; + +RCL_LOCAL +rcl_ret_t +rcl_remap_name( + const rcl_arguments_t * local_arguments, + const rcl_arguments_t * global_arguments, + rcl_remap_type_t type_bitmask, + const char * name, + const char * node_name, + const char * node_namespace, + const rcutils_string_map_t * substitutions, + rcl_allocator_t allocator, + char ** output_name); + +#ifdef __cplusplus +} +#endif + +#endif // RCL__REMAP_IMPL_H_ diff --git a/src/rcl/rmw_implementation_identifier_check.c b/src/rcl/rmw_implementation_identifier_check.c new file mode 100644 index 0000000..235e3a4 --- /dev/null +++ b/src/rcl/rmw_implementation_identifier_check.c @@ -0,0 +1,178 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include +#include +#include + +#include "rcl/allocator.h" +#include "rcl/error_handling.h" +#include "rcutils/env.h" +#include "rcutils/logging_macros.h" +#include "rcutils/strdup.h" +#include "rmw/rmw.h" + +#include "rcl/types.h" + +#include "rcl/rmw_implementation_identifier_check.h" + +// Extracted this portable method of doing a "shared library constructor" from SO: +// http://stackoverflow.com/a/2390626/671658 +// Initializer/finalizer sample for MSVC and GCC/Clang. +// 2010-2016 Joe Lowe. Released into the public domain. +#if defined(_MSC_VER) + #pragma section(".CRT$XCU", read) + #define INITIALIZER2_(f, p) \ + static void f(void); \ + __declspec(allocate(".CRT$XCU"))void(*f ## _)(void) = f; \ + __pragma(comment(linker, "/include:" p #f "_")) \ + static void f(void) + #ifdef _WIN64 + #define INITIALIZER(f) INITIALIZER2_(f, "") + #else + #define INITIALIZER(f) INITIALIZER2_(f, "_") + #endif +#else + #define INITIALIZER(f) \ + static void f(void) __attribute__((constructor)); \ + static void f(void) +#endif + +rcl_ret_t rcl_rmw_implementation_identifier_check(void) +{ + // If the environment variable RMW_IMPLEMENTATION is set, or + // the environment variable RCL_ASSERT_RMW_ID_MATCHES is set, + // check that the result of `rmw_get_implementation_identifier` matches. + rcl_ret_t ret = RCL_RET_OK; + rcl_allocator_t allocator = rcl_get_default_allocator(); + char * expected_rmw_impl = NULL; + const char * expected_rmw_impl_env = NULL; + const char * get_env_error_str = rcutils_get_env( + RMW_IMPLEMENTATION_ENV_VAR_NAME, + &expected_rmw_impl_env); + if (NULL != get_env_error_str) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Error getting env var '" RCUTILS_STRINGIFY(RMW_IMPLEMENTATION_ENV_VAR_NAME) "': %s\n", + get_env_error_str); + return RCL_RET_ERROR; + } + if (strlen(expected_rmw_impl_env) > 0) { + // Copy the environment variable so it doesn't get over-written by the next getenv call. + expected_rmw_impl = rcutils_strdup(expected_rmw_impl_env, allocator); + if (!expected_rmw_impl) { + RCL_SET_ERROR_MSG("allocation failed"); + return RCL_RET_BAD_ALLOC; + } + } + + char * asserted_rmw_impl = NULL; + const char * asserted_rmw_impl_env = NULL; + get_env_error_str = rcutils_get_env( + RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, &asserted_rmw_impl_env); + if (NULL != get_env_error_str) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Error getting env var '" + RCUTILS_STRINGIFY(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME) "': %s\n", + get_env_error_str); + ret = RCL_RET_ERROR; + goto cleanup; + } + if (strlen(asserted_rmw_impl_env) > 0) { + // Copy the environment variable so it doesn't get over-written by the next getenv call. + asserted_rmw_impl = rcutils_strdup(asserted_rmw_impl_env, allocator); + if (!asserted_rmw_impl) { + RCL_SET_ERROR_MSG("allocation failed"); + ret = RCL_RET_BAD_ALLOC; + goto cleanup; + } + } + + // If both environment variables are set, and they do not match, print an error and exit. + if (expected_rmw_impl && asserted_rmw_impl && strcmp(expected_rmw_impl, asserted_rmw_impl) != 0) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Values of RMW_IMPLEMENTATION ('%s') and RCL_ASSERT_RMW_ID_MATCHES ('%s') environment " + "variables do not match, exiting with %d.", + expected_rmw_impl, asserted_rmw_impl, RCL_RET_ERROR + ); + ret = RCL_RET_ERROR; + goto cleanup; + } + + // Collapse the expected_rmw_impl and asserted_rmw_impl variables so only expected_rmw_impl needs + // to be used from now on. + if (expected_rmw_impl && asserted_rmw_impl) { + // The strings at this point must be equal. + // No need for asserted_rmw_impl anymore, free the memory. + allocator.deallocate(asserted_rmw_impl, allocator.state); + asserted_rmw_impl = NULL; + } else { + // One or none are set. + // If asserted_rmw_impl has contents, move it over to expected_rmw_impl. + if (asserted_rmw_impl) { + expected_rmw_impl = asserted_rmw_impl; + asserted_rmw_impl = NULL; + } + } + + // If either environment variable is set, and it does not match, print an error and exit. + if (expected_rmw_impl) { + const char * actual_rmw_impl_id = rmw_get_implementation_identifier(); + const rcutils_error_string_t rmw_error_msg = rcl_get_error_string(); + rcl_reset_error(); + if (!actual_rmw_impl_id) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Error getting RMW implementation identifier / RMW implementation not installed " + "(expected identifier of '%s'), with error message '%s', exiting with %d.", + expected_rmw_impl, + rmw_error_msg.str, + RCL_RET_ERROR + ); + ret = RCL_RET_ERROR; + goto cleanup; + } + if (strcmp(actual_rmw_impl_id, expected_rmw_impl) != 0) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Expected RMW implementation identifier of '%s' but instead found '%s', exiting with %d.", + expected_rmw_impl, + actual_rmw_impl_id, + RCL_RET_MISMATCHED_RMW_ID + ); + ret = RCL_RET_MISMATCHED_RMW_ID; + goto cleanup; + } + } + ret = RCL_RET_OK; +// fallthrough +cleanup: + allocator.deallocate(expected_rmw_impl, allocator.state); + allocator.deallocate(asserted_rmw_impl, allocator.state); + return ret; +} + +INITIALIZER(initialize) { + rcl_ret_t ret = rcl_rmw_implementation_identifier_check(); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "%s\n", rcl_get_error_string().str); + exit(ret); + } +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/security.c b/src/rcl/security.c new file mode 100644 index 0000000..256a345 --- /dev/null +++ b/src/rcl/security.c @@ -0,0 +1,212 @@ +// Copyright 2018-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 "rcl/security.h" + +#include + +#include "rcl/error_handling.h" + +#include "rcutils/env.h" +#include "rcutils/filesystem.h" +#include "rcutils/logging_macros.h" +#include "rcutils/strdup.h" + +#include "rmw/security_options.h" + +rcl_ret_t +rcl_get_security_options_from_environment( + const char * name, + const rcutils_allocator_t * allocator, + rmw_security_options_t * security_options) +{ + bool use_security = false; + rcl_ret_t ret = rcl_security_enabled(&use_security); + if (RCL_RET_OK != ret) { + return ret; + } + + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Using security: %s", use_security ? "true" : "false"); + + if (!use_security) { + security_options->enforce_security = RMW_SECURITY_ENFORCEMENT_PERMISSIVE; + return RCL_RET_OK; + } + + ret = rcl_get_enforcement_policy(&security_options->enforce_security); + if (RCL_RET_OK != ret) { + return ret; + } + + // File discovery magic here + char * secure_root = rcl_get_secure_root(name, allocator); + if (secure_root) { + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "Found security directory: %s", secure_root); + security_options->security_root_path = secure_root; + } else { + if (RMW_SECURITY_ENFORCEMENT_ENFORCE == security_options->enforce_security) { + return RCL_RET_ERROR; + } + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_security_enabled(bool * use_security) +{ + const char * ros_security_enable = NULL; + const char * get_env_error_str = NULL; + + RCL_CHECK_ARGUMENT_FOR_NULL(use_security, RCL_RET_INVALID_ARGUMENT); + + get_env_error_str = rcutils_get_env(ROS_SECURITY_ENABLE_VAR_NAME, &ros_security_enable); + if (NULL != get_env_error_str) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Error getting env var '" RCUTILS_STRINGIFY(ROS_SECURITY_ENABLE_VAR_NAME) "': %s\n", + get_env_error_str); + return RCL_RET_ERROR; + } + + *use_security = (0 == strcmp(ros_security_enable, "true")); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_get_enforcement_policy(rmw_security_enforcement_policy_t * policy) +{ + const char * ros_enforce_security = NULL; + const char * get_env_error_str = NULL; + + RCL_CHECK_ARGUMENT_FOR_NULL(policy, RCL_RET_INVALID_ARGUMENT); + + get_env_error_str = rcutils_get_env(ROS_SECURITY_STRATEGY_VAR_NAME, &ros_enforce_security); + if (NULL != get_env_error_str) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "Error getting env var '" RCUTILS_STRINGIFY(ROS_SECURITY_STRATEGY_VAR_NAME) "': %s\n", + get_env_error_str); + return RCL_RET_ERROR; + } + + *policy = (0 == strcmp(ros_enforce_security, "Enforce")) ? + RMW_SECURITY_ENFORCEMENT_ENFORCE : RMW_SECURITY_ENFORCEMENT_PERMISSIVE; + return RCL_RET_OK; +} + +char * exact_match_lookup( + const char * name, + const char * ros_secure_keystore_env, + const rcl_allocator_t * allocator) +{ + // Perform an exact match for the enclave name in directory . + char * secure_root = NULL; + char * enclaves_dir = NULL; + enclaves_dir = rcutils_join_path(ros_secure_keystore_env, "enclaves", *allocator); + // "/" case when root namespace is explicitly passed in + if (0 == strcmp(name, "/")) { + secure_root = enclaves_dir; + } else { + char * relative_path = NULL; + // Get native path, ignore the leading forward slash + // TODO(ros2team): remove the hard-coded length, use the length of the root namespace instead + relative_path = rcutils_to_native_path(name + 1, *allocator); + secure_root = rcutils_join_path(enclaves_dir, relative_path, *allocator); + allocator->deallocate(relative_path, allocator->state); + allocator->deallocate(enclaves_dir, allocator->state); + } + return secure_root; +} + +static const char * +dupenv(const char * name, const rcl_allocator_t * allocator, char ** value) +{ + const char * buffer = NULL; + const char * error = rcutils_get_env(name, &buffer); + if (NULL != error) { + return error; + } + *value = NULL; + if (0 != strcmp("", buffer)) { + *value = rcutils_strdup(buffer, *allocator); + if (NULL == *value) { + return "string duplication failed"; + } + } + return NULL; +} + +char * rcl_get_secure_root( + const char * name, + const rcl_allocator_t * allocator) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(name, NULL); + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "allocator is invalid", return NULL); + + char * secure_root = NULL; + char * ros_secure_keystore_env = NULL; + char * ros_secure_enclave_override_env = NULL; + + // check keystore environment variable + const char * error = + dupenv(ROS_SECURITY_KEYSTORE_VAR_NAME, allocator, &ros_secure_keystore_env); + if (NULL != error) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "failed to get %s: %s", ROS_SECURITY_KEYSTORE_VAR_NAME, error); + return NULL; + } + + if (NULL == ros_secure_keystore_env) { + return NULL; // environment variable was empty + } + + // check enclave override environment variable + error = dupenv(ROS_SECURITY_ENCLAVE_OVERRIDE, allocator, &ros_secure_enclave_override_env); + if (NULL != error) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "failed to get %s: %s", ROS_SECURITY_ENCLAVE_OVERRIDE, error); + goto leave_rcl_get_secure_root; + } + + // given usable environment variables, overwrite with next lookup + if (NULL != ros_secure_enclave_override_env) { + secure_root = exact_match_lookup( + ros_secure_enclave_override_env, + ros_secure_keystore_env, + allocator); + } else { + secure_root = exact_match_lookup( + name, + ros_secure_keystore_env, + allocator); + } + + if (NULL == secure_root) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "SECURITY ERROR: unable to find a folder matching the name '%s' in '%s'. ", + name, ros_secure_keystore_env); + goto leave_rcl_get_secure_root; + } + + if (!rcutils_is_directory(secure_root)) { + RCL_SET_ERROR_MSG_WITH_FORMAT_STRING( + "SECURITY ERROR: directory '%s' does not exist.", secure_root); + allocator->deallocate(secure_root, allocator->state); + secure_root = NULL; + } + +leave_rcl_get_secure_root: + allocator->deallocate(ros_secure_enclave_override_env, allocator->state); + allocator->deallocate(ros_secure_keystore_env, allocator->state); + return secure_root; +} diff --git a/src/rcl/service.c b/src/rcl/service.c new file mode 100644 index 0000000..e1bdb01 --- /dev/null +++ b/src/rcl/service.c @@ -0,0 +1,368 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/service.h" + +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/node.h" +#include "rcutils/logging_macros.h" +#include "rcutils/macros.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "tracetools/tracetools.h" + +struct rcl_service_impl_s +{ + rcl_service_options_t options; + rmw_qos_profile_t actual_request_subscription_qos; + rmw_qos_profile_t actual_response_publisher_qos; + rmw_service_t * rmw_handle; +}; + +rcl_service_t +rcl_get_zero_initialized_service() +{ + static rcl_service_t null_service = {0}; + return null_service; +} + +rcl_ret_t +rcl_service_init( + rcl_service_t * service, + const rcl_node_t * node, + const rosidl_service_type_support_t * type_support, + const char * service_name, + const rcl_service_options_t * options) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ALREADY_INIT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_BAD_ALLOC); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SERVICE_NAME_INVALID); + + rcl_ret_t fail_ret = RCL_RET_ERROR; + + // Check options and allocator first, so the allocator can be used in errors. + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t * allocator = (rcl_allocator_t *)&options->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_INVALID_ARGUMENT); + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(service_name, RCL_RET_INVALID_ARGUMENT); + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing service for service name '%s'", service_name); + if (service->impl) { + RCL_SET_ERROR_MSG("service already initialized, or memory was unintialized"); + return RCL_RET_ALREADY_INIT; + } + + // Expand and remap the given service name. + char * remapped_service_name = NULL; + rcl_ret_t ret = rcl_node_resolve_name( + node, + service_name, + *allocator, + true, + false, + &remapped_service_name); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_SERVICE_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { + ret = RCL_RET_SERVICE_NAME_INVALID; + } else if (ret != RCL_RET_BAD_ALLOC) { + ret = RCL_RET_ERROR; + } + goto cleanup; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Expanded and remapped service name '%s'", remapped_service_name); + + // Allocate space for the implementation struct. + service->impl = (rcl_service_impl_t *)allocator->allocate( + sizeof(rcl_service_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + service->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup); + + if (RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL == options->qos.durability) { + RCUTILS_LOG_WARN_NAMED( + ROS_PACKAGE_NAME, + "Warning: Setting QoS durability to 'transient local' for service servers " + "can cause them to receive requests from clients that have since terminated."); + } + // Fill out implementation struct. + // rmw handle (create rmw service) + // TODO(wjwwood): pass along the allocator to rmw when it supports it + service->impl->rmw_handle = rmw_create_service( + rcl_node_get_rmw_handle(node), + type_support, + remapped_service_name, + &options->qos); + if (!service->impl->rmw_handle) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + goto fail; + } + // get actual qos, and store it + rmw_ret_t rmw_ret = rmw_service_request_subscription_get_actual_qos( + service->impl->rmw_handle, + &service->impl->actual_request_subscription_qos); + + if (RMW_RET_OK != rmw_ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + goto fail; + } + + rmw_ret = rmw_service_response_publisher_get_actual_qos( + service->impl->rmw_handle, + &service->impl->actual_response_publisher_qos); + + if (RMW_RET_OK != rmw_ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + goto fail; + } + + // ROS specific namespacing conventions is not retrieved by get_actual_qos + service->impl->actual_request_subscription_qos.avoid_ros_namespace_conventions = + options->qos.avoid_ros_namespace_conventions; + service->impl->actual_response_publisher_qos.avoid_ros_namespace_conventions = + options->qos.avoid_ros_namespace_conventions; + + // options + service->impl->options = *options; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service initialized"); + ret = RCL_RET_OK; + TRACEPOINT( + rcl_service_init, + (const void *)service, + (const void *)node, + (const void *)service->impl->rmw_handle, + remapped_service_name); + goto cleanup; +fail: + if (service->impl) { + allocator->deallocate(service->impl, allocator->state); + service->impl = NULL; + } + ret = fail_ret; + // Fall through to clean up +cleanup: + allocator->deallocate(remapped_service_name, allocator->state); + return ret; +} + +rcl_ret_t +rcl_service_fini(rcl_service_t * service, rcl_node_t * node) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SERVICE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing service"); + RCL_CHECK_ARGUMENT_FOR_NULL(service, RCL_RET_SERVICE_INVALID); + if (!rcl_node_is_valid_except_context(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + + rcl_ret_t result = RCL_RET_OK; + if (service->impl) { + rcl_allocator_t allocator = service->impl->options.allocator; + rmw_node_t * rmw_node = rcl_node_get_rmw_handle(node); + if (!rmw_node) { + return RCL_RET_INVALID_ARGUMENT; + } + rmw_ret_t ret = rmw_destroy_service(rmw_node, service->impl->rmw_handle); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + result = RCL_RET_ERROR; + } + allocator.deallocate(service->impl, allocator.state); + service->impl = NULL; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service finalized"); + return result; +} + +rcl_service_options_t +rcl_service_get_default_options() +{ + // !!! MAKE SURE THAT CHANGES TO THESE DEFAULTS ARE REFLECTED IN THE HEADER DOC STRING + static rcl_service_options_t default_options; + // Must set the allocator and qos after because they are not a compile time constant. + default_options.qos = rmw_qos_profile_services_default; + default_options.allocator = rcl_get_default_allocator(); + return default_options; +} + +const char * +rcl_service_get_service_name(const rcl_service_t * service) +{ + const rcl_service_options_t * options = rcl_service_get_options(service); + if (!options) { + return NULL; + } + RCL_CHECK_FOR_NULL_WITH_MSG(service->impl->rmw_handle, "service is invalid", return NULL); + return service->impl->rmw_handle->service_name; +} + +#define _service_get_options(service) & service->impl->options + +const rcl_service_options_t * +rcl_service_get_options(const rcl_service_t * service) +{ + if (!rcl_service_is_valid(service)) { + return NULL; // error already set + } + return _service_get_options(service); +} + +rmw_service_t * +rcl_service_get_rmw_handle(const rcl_service_t * service) +{ + if (!rcl_service_is_valid(service)) { + return NULL; // error already set + } + return service->impl->rmw_handle; +} + +rcl_ret_t +rcl_take_request_with_info( + const rcl_service_t * service, + rmw_service_info_t * request_header, + void * ros_request) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Service server taking service request"); + if (!rcl_service_is_valid(service)) { + return RCL_RET_SERVICE_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_request, RCL_RET_INVALID_ARGUMENT); + const rcl_service_options_t * options = rcl_service_get_options(service); + RCL_CHECK_FOR_NULL_WITH_MSG(options, "Failed to get service options", return RCL_RET_ERROR); + + bool taken = false; + rmw_ret_t ret = rmw_take_request( + service->impl->rmw_handle, request_header, ros_request, &taken); + if (RMW_RET_OK != ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + if (RMW_RET_BAD_ALLOC == ret) { + return RCL_RET_BAD_ALLOC; + } + return RCL_RET_ERROR; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Service take request succeeded: %s", taken ? "true" : "false"); + if (!taken) { + return RCL_RET_SERVICE_TAKE_FAILED; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_take_request( + const rcl_service_t * service, + rmw_request_id_t * request_header, + void * ros_request) +{ + rmw_service_info_t header; + header.request_id = *request_header; + rcl_ret_t ret = rcl_take_request_with_info(service, &header, ros_request); + *request_header = header.request_id; + return ret; +} + +rcl_ret_t +rcl_send_response( + const rcl_service_t * service, + rmw_request_id_t * request_header, + void * ros_response) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Sending service response"); + if (!rcl_service_is_valid(service)) { + return RCL_RET_SERVICE_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(request_header, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(ros_response, RCL_RET_INVALID_ARGUMENT); + const rcl_service_options_t * options = rcl_service_get_options(service); + RCL_CHECK_FOR_NULL_WITH_MSG(options, "Failed to get service options", return RCL_RET_ERROR); + + if (rmw_send_response( + service->impl->rmw_handle, request_header, ros_response) != RMW_RET_OK) + { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + return RCL_RET_OK; +} + +bool +rcl_service_is_valid(const rcl_service_t * service) +{ + RCL_CHECK_FOR_NULL_WITH_MSG(service, "service pointer is invalid", return false); + RCL_CHECK_FOR_NULL_WITH_MSG( + service->impl, "service's implementation is invalid", return false); + RCL_CHECK_FOR_NULL_WITH_MSG( + service->impl->rmw_handle, "service's rmw handle is invalid", return false); + return true; +} + +const rmw_qos_profile_t * +rcl_service_request_subscription_get_actual_qos(const rcl_service_t * service) +{ + if (!rcl_service_is_valid(service)) { + return NULL; + } + return &service->impl->actual_request_subscription_qos; +} + +const rmw_qos_profile_t * +rcl_service_response_publisher_get_actual_qos(const rcl_service_t * service) +{ + if (!rcl_service_is_valid(service)) { + return NULL; + } + return &service->impl->actual_response_publisher_qos; +} + +rcl_ret_t +rcl_service_set_on_new_request_callback( + const rcl_service_t * service, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_service_is_valid(service)) { + // error state already set + return RCL_RET_INVALID_ARGUMENT; + } + + return rmw_service_set_on_new_request_callback( + service->impl->rmw_handle, + callback, + user_data); +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/subscription.c b/src/rcl/subscription.c new file mode 100644 index 0000000..fd5984d --- /dev/null +++ b/src/rcl/subscription.c @@ -0,0 +1,767 @@ +// Copyright 2015 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/subscription.h" + +#include + +#include "rcl/error_handling.h" +#include "rcl/node.h" +#include "rcutils/logging_macros.h" +#include "rcutils/strdup.h" +#include "rcutils/types/string_array.h" +#include "rmw/error_handling.h" +#include "rmw/subscription_content_filter_options.h" +#include "rmw/validate_full_topic_name.h" +#include "tracetools/tracetools.h" + +#include "./common.h" +#include "./subscription_impl.h" + + +rcl_subscription_t +rcl_get_zero_initialized_subscription() +{ + static rcl_subscription_t null_subscription = {0}; + return null_subscription; +} + +rcl_ret_t +rcl_subscription_init( + rcl_subscription_t * subscription, + const rcl_node_t * node, + const rosidl_message_type_support_t * type_support, + const char * topic_name, + const rcl_subscription_options_t * options +) +{ + rcl_ret_t fail_ret = RCL_RET_ERROR; + + // Check options and allocator first, so the allocator can be used in errors. + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t * allocator = (rcl_allocator_t *)&options->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_INVALID_ARGUMENT); + if (!rcl_node_is_valid(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(type_support, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing subscription for topic name '%s'", topic_name); + if (subscription->impl) { + RCL_SET_ERROR_MSG("subscription already initialized, or memory was uninitialized"); + return RCL_RET_ALREADY_INIT; + } + + // Expand and remap the given topic name. + char * remapped_topic_name = NULL; + rcl_ret_t ret = rcl_node_resolve_name( + node, + topic_name, + *allocator, + false, + false, + &remapped_topic_name); + if (ret != RCL_RET_OK) { + if (ret == RCL_RET_TOPIC_NAME_INVALID || ret == RCL_RET_UNKNOWN_SUBSTITUTION) { + ret = RCL_RET_TOPIC_NAME_INVALID; + } else if (ret != RCL_RET_BAD_ALLOC) { + ret = RCL_RET_ERROR; + } + goto cleanup; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Expanded and remapped topic name '%s'", remapped_topic_name); + + // Allocate memory for the implementation struct. + subscription->impl = (rcl_subscription_impl_t *)allocator->zero_allocate( + 1, sizeof(rcl_subscription_impl_t), allocator->state); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl, "allocating memory failed", ret = RCL_RET_BAD_ALLOC; goto cleanup); + // Fill out the implemenation struct. + // rmw_handle + // TODO(wjwwood): pass allocator once supported in rmw api. + subscription->impl->rmw_handle = rmw_create_subscription( + rcl_node_get_rmw_handle(node), + type_support, + remapped_topic_name, + &(options->qos), + &(options->rmw_subscription_options)); + if (!subscription->impl->rmw_handle) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + goto fail; + } + // get actual qos, and store it + rmw_ret_t rmw_ret = rmw_subscription_get_actual_qos( + subscription->impl->rmw_handle, + &subscription->impl->actual_qos); + if (RMW_RET_OK != rmw_ret) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + goto fail; + } + subscription->impl->actual_qos.avoid_ros_namespace_conventions = + options->qos.avoid_ros_namespace_conventions; + // options + subscription->impl->options = *options; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription initialized"); + ret = RCL_RET_OK; + TRACEPOINT( + rcl_subscription_init, + (const void *)subscription, + (const void *)node, + (const void *)subscription->impl->rmw_handle, + remapped_topic_name, + options->qos.depth); + goto cleanup; +fail: + if (subscription->impl) { + if (subscription->impl->rmw_handle) { + rmw_ret_t rmw_fail_ret = rmw_destroy_subscription( + rcl_node_get_rmw_handle(node), subscription->impl->rmw_handle); + if (RMW_RET_OK != rmw_fail_ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } + } + + ret = rcl_subscription_options_fini(&subscription->impl->options); + if (RCL_RET_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rmw_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + } + + allocator->deallocate(subscription->impl, allocator->state); + subscription->impl = NULL; + } + ret = fail_ret; + // Fall through to cleanup +cleanup: + allocator->deallocate(remapped_topic_name, allocator->state); + return ret; +} + +rcl_ret_t +rcl_subscription_fini(rcl_subscription_t * subscription, rcl_node_t * node) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_NODE_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Finalizing subscription"); + rcl_ret_t result = RCL_RET_OK; + RCL_CHECK_ARGUMENT_FOR_NULL(subscription, RCL_RET_SUBSCRIPTION_INVALID); + if (!rcl_node_is_valid_except_context(node)) { + return RCL_RET_NODE_INVALID; // error already set + } + if (subscription->impl) { + rcl_allocator_t allocator = subscription->impl->options.allocator; + rmw_node_t * rmw_node = rcl_node_get_rmw_handle(node); + if (!rmw_node) { + return RCL_RET_INVALID_ARGUMENT; + } + rmw_ret_t ret = + rmw_destroy_subscription(rmw_node, subscription->impl->rmw_handle); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + result = RCL_RET_ERROR; + } + rcl_ret_t rcl_ret = rcl_subscription_options_fini(&subscription->impl->options); + if (RCL_RET_OK != rcl_ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR(rcl_get_error_string().str); + RCUTILS_SAFE_FWRITE_TO_STDERR("\n"); + result = RCL_RET_ERROR; + } + + allocator.deallocate(subscription->impl, allocator.state); + subscription->impl = NULL; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription finalized"); + return result; +} + +rcl_subscription_options_t +rcl_subscription_get_default_options() +{ + // !!! MAKE SURE THAT CHANGES TO THESE DEFAULTS ARE REFLECTED IN THE HEADER DOC STRING + static rcl_subscription_options_t default_options; + // Must set these after declaration because they are not a compile time constants. + default_options.qos = rmw_qos_profile_default; + default_options.allocator = rcl_get_default_allocator(); + default_options.rmw_subscription_options = rmw_get_default_subscription_options(); + return default_options; +} + +rcl_ret_t +rcl_subscription_options_fini(rcl_subscription_options_t * option) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(option, RCL_RET_INVALID_ARGUMENT); + // fini rmw_subscription_options_t + const rcl_allocator_t * allocator = &option->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + if (option->rmw_subscription_options.content_filter_options) { + rmw_ret_t ret = rmw_subscription_content_filter_options_fini( + option->rmw_subscription_options.content_filter_options, allocator); + if (RCUTILS_RET_OK != ret) { + RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to fini content filter options.\n"); + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + allocator->deallocate( + option->rmw_subscription_options.content_filter_options, allocator->state); + option->rmw_subscription_options.content_filter_options = NULL; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_subscription_options_set_content_filter_options( + const char * filter_expression, + size_t expression_parameters_argc, + const char * expression_parameter_argv[], + rcl_subscription_options_t * options) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(filter_expression, RCL_RET_INVALID_ARGUMENT); + if (expression_parameters_argc > 100) { + RCL_SET_ERROR_MSG("The maximum of expression parameters argument number is 100"); + return RCL_RET_INVALID_ARGUMENT; + } + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &options->allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + rcl_ret_t ret; + rmw_ret_t rmw_ret; + rmw_subscription_content_filter_options_t * original_content_filter_options = + options->rmw_subscription_options.content_filter_options; + rmw_subscription_content_filter_options_t content_filter_options_backup = + rmw_get_zero_initialized_content_filter_options(); + + if (original_content_filter_options) { + // make a backup, restore the data if failure happened + rmw_ret = rmw_subscription_content_filter_options_copy( + original_content_filter_options, + allocator, + &content_filter_options_backup + ); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + } else { + options->rmw_subscription_options.content_filter_options = + allocator->allocate( + sizeof(rmw_subscription_content_filter_options_t), allocator->state); + if (!options->rmw_subscription_options.content_filter_options) { + RCL_SET_ERROR_MSG("failed to allocate memory"); + return RCL_RET_BAD_ALLOC; + } + *options->rmw_subscription_options.content_filter_options = + rmw_get_zero_initialized_content_filter_options(); + } + + rmw_ret = rmw_subscription_content_filter_options_set( + filter_expression, + expression_parameters_argc, + expression_parameter_argv, + allocator, + options->rmw_subscription_options.content_filter_options + ); + + if (rmw_ret != RMW_RET_OK) { + ret = rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + goto failed; + } + + rmw_ret = rmw_subscription_content_filter_options_fini( + &content_filter_options_backup, + allocator + ); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + + return RMW_RET_OK; + +failed: + + if (original_content_filter_options == NULL) { + if (options->rmw_subscription_options.content_filter_options) { + rmw_ret = rmw_subscription_content_filter_options_fini( + options->rmw_subscription_options.content_filter_options, + allocator + ); + + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + + allocator->deallocate( + options->rmw_subscription_options.content_filter_options, allocator->state); + options->rmw_subscription_options.content_filter_options = NULL; + } + } else { + rmw_ret = rmw_subscription_content_filter_options_copy( + &content_filter_options_backup, + allocator, + options->rmw_subscription_options.content_filter_options + ); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + + rmw_ret = rmw_subscription_content_filter_options_fini( + &content_filter_options_backup, + allocator + ); + if (rmw_ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); + } + } + + return ret; +} + +rcl_subscription_content_filter_options_t +rcl_get_zero_initialized_subscription_content_filter_options() +{ + return (const rcl_subscription_content_filter_options_t) { + .rmw_subscription_content_filter_options = + rmw_get_zero_initialized_content_filter_options() + }; // NOLINT(readability/braces): false positive +} + +rcl_ret_t +rcl_subscription_content_filter_options_init( + const rcl_subscription_t * subscription, + const char * filter_expression, + size_t expression_parameters_argc, + const char * expression_parameter_argv[], + rcl_subscription_content_filter_options_t * options) +{ + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &subscription->impl->options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + if (expression_parameters_argc > 100) { + RCL_SET_ERROR_MSG("The maximum of expression parameters argument number is 100"); + return RCL_RET_INVALID_ARGUMENT; + } + + rmw_ret_t rmw_ret = rmw_subscription_content_filter_options_init( + filter_expression, + expression_parameters_argc, + expression_parameter_argv, + allocator, + &options->rmw_subscription_content_filter_options + ); + + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_subscription_content_filter_options_set( + const rcl_subscription_t * subscription, + const char * filter_expression, + size_t expression_parameters_argc, + const char * expression_parameter_argv[], + rcl_subscription_content_filter_options_t * options) +{ + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; + } + if (expression_parameters_argc > 100) { + RCL_SET_ERROR_MSG("The maximum of expression parameters argument number is 100"); + return RCL_RET_INVALID_ARGUMENT; + } + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &subscription->impl->options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + rmw_ret_t ret = rmw_subscription_content_filter_options_set( + filter_expression, + expression_parameters_argc, + expression_parameter_argv, + allocator, + &options->rmw_subscription_content_filter_options + ); + return rcl_convert_rmw_ret_to_rcl_ret(ret); +} + +rcl_ret_t +rcl_subscription_content_filter_options_fini( + const rcl_subscription_t * subscription, + rcl_subscription_content_filter_options_t * options) +{ + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + const rcl_allocator_t * allocator = &subscription->impl->options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + rmw_ret_t ret = rmw_subscription_content_filter_options_fini( + &options->rmw_subscription_content_filter_options, + allocator + ); + + return rcl_convert_rmw_ret_to_rcl_ret(ret); +} + +bool +rcl_subscription_is_cft_enabled(const rcl_subscription_t * subscription) +{ + if (!rcl_subscription_is_valid(subscription)) { + return false; + } + return subscription->impl->rmw_handle->is_cft_enabled; +} + +rcl_ret_t +rcl_subscription_set_content_filter( + const rcl_subscription_t * subscription, + const rcl_subscription_content_filter_options_t * options +) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; + } + + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t ret = rmw_subscription_set_content_filter( + subscription->impl->rmw_handle, + &options->rmw_subscription_content_filter_options); + + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + + // copy options into subscription_options + const rmw_subscription_content_filter_options_t * content_filter_options = + &options->rmw_subscription_content_filter_options; + return rcl_subscription_options_set_content_filter_options( + content_filter_options->filter_expression, + content_filter_options->expression_parameters.size, + (const char **)content_filter_options->expression_parameters.data, + &subscription->impl->options + ); +} + +rcl_ret_t +rcl_subscription_get_content_filter( + const rcl_subscription_t * subscription, + rcl_subscription_content_filter_options_t * options +) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(options, RCL_RET_INVALID_ARGUMENT); + rcl_allocator_t * allocator = &subscription->impl->options.allocator; + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + + rmw_ret_t rmw_ret = rmw_subscription_get_content_filter( + subscription->impl->rmw_handle, + allocator, + &options->rmw_subscription_content_filter_options); + + return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret); +} + +rcl_ret_t +rcl_take( + const rcl_subscription_t * subscription, + void * ros_message, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation +) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking message"); + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; // error message already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(ros_message, RCL_RET_INVALID_ARGUMENT); + + // If message_info is NULL, use a place holder which can be discarded. + rmw_message_info_t dummy_message_info; + rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info; + *message_info_local = rmw_get_zero_initialized_message_info(); + // Call rmw_take_with_info. + bool taken = false; + rmw_ret_t ret = rmw_take_with_info( + subscription->impl->rmw_handle, ros_message, &taken, message_info_local, allocation); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Subscription take succeeded: %s", taken ? "true" : "false"); + TRACEPOINT(rcl_take, (const void *)ros_message); + if (!taken) { + return RCL_RET_SUBSCRIPTION_TAKE_FAILED; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_take_sequence( + const rcl_subscription_t * subscription, + size_t count, + rmw_message_sequence_t * message_sequence, + rmw_message_info_sequence_t * message_info_sequence, + rmw_subscription_allocation_t * allocation +) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking %zu messages", count); + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; // error message already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(message_sequence, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(message_info_sequence, RCL_RET_INVALID_ARGUMENT); + + if (message_sequence->capacity < count) { + RCL_SET_ERROR_MSG("Insufficient message sequence capacity for requested count"); + return RCL_RET_INVALID_ARGUMENT; + } + + if (message_info_sequence->capacity < count) { + RCL_SET_ERROR_MSG("Insufficient message info sequence capacity for requested count"); + return RCL_RET_INVALID_ARGUMENT; + } + + // Set the sizes to zero to indicate that there are no valid messages + message_sequence->size = 0u; + message_info_sequence->size = 0u; + + size_t taken = 0u; + rmw_ret_t ret = rmw_take_sequence( + subscription->impl->rmw_handle, count, message_sequence, message_info_sequence, &taken, + allocation); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Subscription took %zu messages", taken); + if (0u == taken) { + return RCL_RET_SUBSCRIPTION_TAKE_FAILED; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_take_serialized_message( + const rcl_subscription_t * subscription, + rcl_serialized_message_t * serialized_message, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation +) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking serialized message"); + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(serialized_message, RCL_RET_INVALID_ARGUMENT); + // If message_info is NULL, use a place holder which can be discarded. + rmw_message_info_t dummy_message_info; + rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info; + *message_info_local = rmw_get_zero_initialized_message_info(); + // Call rmw_take_with_info. + bool taken = false; + rmw_ret_t ret = rmw_take_serialized_message_with_info( + subscription->impl->rmw_handle, serialized_message, &taken, message_info_local, allocation); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Subscription serialized take succeeded: %s", taken ? "true" : "false"); + if (!taken) { + return RCL_RET_SUBSCRIPTION_TAKE_FAILED; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_take_loaned_message( + const rcl_subscription_t * subscription, + void ** loaned_message, + rmw_message_info_t * message_info, + rmw_subscription_allocation_t * allocation) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription taking loaned message"); + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message, RCL_RET_INVALID_ARGUMENT); + if (*loaned_message) { + RCL_SET_ERROR_MSG("loaned message is already initialized"); + return RCL_RET_INVALID_ARGUMENT; + } + // If message_info is NULL, use a place holder which can be discarded. + rmw_message_info_t dummy_message_info; + rmw_message_info_t * message_info_local = message_info ? message_info : &dummy_message_info; + *message_info_local = rmw_get_zero_initialized_message_info(); + // Call rmw_take_with_info. + bool taken = false; + rmw_ret_t ret = rmw_take_loaned_message_with_info( + subscription->impl->rmw_handle, loaned_message, &taken, message_info_local, allocation); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Subscription loaned take succeeded: %s", taken ? "true" : "false"); + if (!taken) { + return RCL_RET_SUBSCRIPTION_TAKE_FAILED; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_return_loaned_message_from_subscription( + const rcl_subscription_t * subscription, + void * loaned_message) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Subscription releasing loaned message"); + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; // error already set + } + RCL_CHECK_ARGUMENT_FOR_NULL(loaned_message, RCL_RET_INVALID_ARGUMENT); + return rcl_convert_rmw_ret_to_rcl_ret( + rmw_return_loaned_message_from_subscription( + subscription->impl->rmw_handle, loaned_message)); +} + +const char * +rcl_subscription_get_topic_name(const rcl_subscription_t * subscription) +{ + if (!rcl_subscription_is_valid(subscription)) { + return NULL; // error already set + } + return subscription->impl->rmw_handle->topic_name; +} + +#define _subscription_get_options(subscription) & subscription->impl->options + +const rcl_subscription_options_t * +rcl_subscription_get_options(const rcl_subscription_t * subscription) +{ + if (!rcl_subscription_is_valid(subscription)) { + return NULL; // error already set + } + return _subscription_get_options(subscription); +} + +rmw_subscription_t * +rcl_subscription_get_rmw_handle(const rcl_subscription_t * subscription) +{ + if (!rcl_subscription_is_valid(subscription)) { + return NULL; // error already set + } + return subscription->impl->rmw_handle; +} + +bool +rcl_subscription_is_valid(const rcl_subscription_t * subscription) +{ + RCL_CHECK_FOR_NULL_WITH_MSG(subscription, "subscription pointer is invalid", return false); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl, "subscription's implementation is invalid", return false); + RCL_CHECK_FOR_NULL_WITH_MSG( + subscription->impl->rmw_handle, "subscription's rmw handle is invalid", return false); + return true; +} + +rmw_ret_t +rcl_subscription_get_publisher_count( + const rcl_subscription_t * subscription, + size_t * publisher_count) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_SUBSCRIPTION_INVALID); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + + if (!rcl_subscription_is_valid(subscription)) { + return RCL_RET_SUBSCRIPTION_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(publisher_count, RCL_RET_INVALID_ARGUMENT); + rmw_ret_t ret = rmw_subscription_count_matched_publishers( + subscription->impl->rmw_handle, publisher_count); + + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + return RCL_RET_OK; +} + +const rmw_qos_profile_t * +rcl_subscription_get_actual_qos(const rcl_subscription_t * subscription) +{ + if (!rcl_subscription_is_valid(subscription)) { + return NULL; + } + return &subscription->impl->actual_qos; +} + +bool +rcl_subscription_can_loan_messages(const rcl_subscription_t * subscription) +{ + if (!rcl_subscription_is_valid(subscription)) { + return false; // error message already set + } + + bool disable_loaned_message = false; + rcl_ret_t ret = rcl_get_disable_loaned_message(&disable_loaned_message); + if (ret == RCL_RET_OK && disable_loaned_message) { + return false; + } + + return subscription->impl->rmw_handle->can_loan_messages; +} + +rcl_ret_t +rcl_subscription_set_on_new_message_callback( + const rcl_subscription_t * subscription, + rcl_event_callback_t callback, + const void * user_data) +{ + if (!rcl_subscription_is_valid(subscription)) { + // error state already set + return RCL_RET_INVALID_ARGUMENT; + } + + return rmw_subscription_set_on_new_message_callback( + subscription->impl->rmw_handle, + callback, + user_data); +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/subscription_impl.h b/src/rcl/subscription_impl.h new file mode 100644 index 0000000..0fe962a --- /dev/null +++ b/src/rcl/subscription_impl.h @@ -0,0 +1,29 @@ +// Copyright 2015 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 RCL__SUBSCRIPTION_IMPL_H_ +#define RCL__SUBSCRIPTION_IMPL_H_ + +#include "rmw/rmw.h" + +#include "rcl/subscription.h" + +struct rcl_subscription_impl_s +{ + rcl_subscription_options_t options; + rmw_qos_profile_t actual_qos; + rmw_subscription_t * rmw_handle; +}; + +#endif // RCL__SUBSCRIPTION_IMPL_H_ diff --git a/src/rcl/time.c b/src/rcl/time.c new file mode 100644 index 0000000..0d9d11a --- /dev/null +++ b/src/rcl/time.c @@ -0,0 +1,465 @@ +// Copyright 2015 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 "rcl/time.h" + +#include +#include + +#include "./common.h" +#include "rcl/allocator.h" +#include "rcl/error_handling.h" +#include "rcutils/macros.h" +#include "rcutils/stdatomic_helper.h" +#include "rcutils/time.h" + +// Internal storage for RCL_ROS_TIME implementation +typedef struct rcl_ros_clock_storage_s +{ + atomic_uint_least64_t current_time; + bool active; +} rcl_ros_clock_storage_t; + +// Implementation only +static rcl_ret_t +rcl_get_steady_time(void * data, rcl_time_point_value_t * current_time) +{ + (void)data; // unused + return rcutils_steady_time_now(current_time); +} + +// Implementation only +static rcl_ret_t +rcl_get_system_time(void * data, rcl_time_point_value_t * current_time) +{ + (void)data; // unused + return rcutils_system_time_now(current_time); +} + +// Internal method for zeroing values on init, assumes clock is valid +static void +rcl_init_generic_clock(rcl_clock_t * clock, rcl_allocator_t * allocator) +{ + clock->type = RCL_CLOCK_UNINITIALIZED; + clock->jump_callbacks = NULL; + clock->num_jump_callbacks = 0u; + clock->get_now = NULL; + clock->data = NULL; + clock->allocator = *allocator; +} + +// The function used to get the current ros time. +// This is in the implementation only +static rcl_ret_t +rcl_get_ros_time(void * data, rcl_time_point_value_t * current_time) +{ + rcl_ros_clock_storage_t * t = (rcl_ros_clock_storage_t *)data; + if (!t->active) { + return rcl_get_system_time(data, current_time); + } + *current_time = rcutils_atomic_load_uint64_t(&(t->current_time)); + return RCL_RET_OK; +} + +bool +rcl_clock_valid(rcl_clock_t * clock) +{ + if (clock == NULL || + clock->type == RCL_CLOCK_UNINITIALIZED || + clock->get_now == NULL) + { + return false; + } + return true; +} + +rcl_ret_t +rcl_clock_init( + rcl_clock_type_t clock_type, rcl_clock_t * clock, + rcl_allocator_t * allocator) +{ + RCL_CHECK_ALLOCATOR_WITH_MSG(allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + switch (clock_type) { + case RCL_CLOCK_UNINITIALIZED: + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + rcl_init_generic_clock(clock, allocator); + return RCL_RET_OK; + case RCL_ROS_TIME: + return rcl_ros_clock_init(clock, allocator); + case RCL_SYSTEM_TIME: + return rcl_system_clock_init(clock, allocator); + case RCL_STEADY_TIME: + return rcl_steady_clock_init(clock, allocator); + default: + return RCL_RET_INVALID_ARGUMENT; + } +} + +static void +rcl_clock_generic_fini( + rcl_clock_t * clock) +{ + // Internal function; assume caller has already checked that clock is valid. + if (clock->num_jump_callbacks > 0) { + clock->num_jump_callbacks = 0; + clock->allocator.deallocate(clock->jump_callbacks, clock->allocator.state); + clock->jump_callbacks = NULL; + } +} + +rcl_ret_t +rcl_clock_fini( + rcl_clock_t * clock) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG( + &clock->allocator, "clock has invalid allocator", return RCL_RET_ERROR); + switch (clock->type) { + case RCL_ROS_TIME: + return rcl_ros_clock_fini(clock); + case RCL_SYSTEM_TIME: + return rcl_system_clock_fini(clock); + case RCL_STEADY_TIME: + return rcl_steady_clock_fini(clock); + case RCL_CLOCK_UNINITIALIZED: + // fall through + default: + return RCL_RET_INVALID_ARGUMENT; + } +} + +rcl_ret_t +rcl_ros_clock_init( + rcl_clock_t * clock, + rcl_allocator_t * allocator) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + rcl_init_generic_clock(clock, allocator); + clock->data = allocator->allocate(sizeof(rcl_ros_clock_storage_t), allocator->state); + if (NULL == clock->data) { + RCL_SET_ERROR_MSG("allocating memory failed"); + return RCL_RET_BAD_ALLOC; + } + + rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data; + // 0 is a special value meaning time has not been set + atomic_init(&(storage->current_time), 0); + storage->active = false; + clock->get_now = rcl_get_ros_time; + clock->type = RCL_ROS_TIME; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_ros_clock_fini( + rcl_clock_t * clock) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + if (clock->type != RCL_ROS_TIME) { + RCL_SET_ERROR_MSG("clock not of type RCL_ROS_TIME"); + return RCL_RET_ERROR; + } + rcl_clock_generic_fini(clock); + clock->allocator.deallocate(clock->data, clock->allocator.state); + clock->data = NULL; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_steady_clock_init( + rcl_clock_t * clock, + rcl_allocator_t * allocator) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + rcl_init_generic_clock(clock, allocator); + clock->get_now = rcl_get_steady_time; + clock->type = RCL_STEADY_TIME; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_steady_clock_fini( + rcl_clock_t * clock) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + if (clock->type != RCL_STEADY_TIME) { + RCL_SET_ERROR_MSG("clock not of type RCL_STEADY_TIME"); + return RCL_RET_ERROR; + } + rcl_clock_generic_fini(clock); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_system_clock_init( + rcl_clock_t * clock, + rcl_allocator_t * allocator) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + rcl_init_generic_clock(clock, allocator); + clock->get_now = rcl_get_system_time; + clock->type = RCL_SYSTEM_TIME; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_system_clock_fini( + rcl_clock_t * clock) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + if (clock->type != RCL_SYSTEM_TIME) { + RCL_SET_ERROR_MSG("clock not of type RCL_SYSTEM_TIME"); + return RCL_RET_ERROR; + } + rcl_clock_generic_fini(clock); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_difference_times( + const rcl_time_point_t * start, const rcl_time_point_t * finish, rcl_duration_t * delta) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(start, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(finish, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(delta, RCL_RET_INVALID_ARGUMENT); + if (start->clock_type != finish->clock_type) { + RCL_SET_ERROR_MSG("Cannot difference between time points with clocks types."); + return RCL_RET_ERROR; + } + if (finish->nanoseconds < start->nanoseconds) { + rcl_time_point_value_t intermediate = start->nanoseconds - finish->nanoseconds; + delta->nanoseconds = -1 * (int64_t) intermediate; + } else { + delta->nanoseconds = (int64_t)(finish->nanoseconds - start->nanoseconds); + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_clock_get_now(rcl_clock_t * clock, rcl_time_point_value_t * time_point_value) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(time_point_value, RCL_RET_INVALID_ARGUMENT); + if (clock->type && clock->get_now) { + return clock->get_now(clock->data, time_point_value); + } + RCL_SET_ERROR_MSG("Clock is not initialized or does not have get_now registered."); + return RCL_RET_ERROR; +} + +static void +rcl_clock_call_callbacks( + rcl_clock_t * clock, const rcl_time_jump_t * time_jump, bool before_jump) +{ + // Internal function; assume parameters are valid. + bool is_clock_change = time_jump->clock_change == RCL_ROS_TIME_ACTIVATED || + time_jump->clock_change == RCL_ROS_TIME_DEACTIVATED; + for (size_t cb_idx = 0; cb_idx < clock->num_jump_callbacks; ++cb_idx) { + rcl_jump_callback_info_t * info = &(clock->jump_callbacks[cb_idx]); + if ( + (is_clock_change && info->threshold.on_clock_change) || + (info->threshold.min_backward.nanoseconds < 0 && + time_jump->delta.nanoseconds <= info->threshold.min_backward.nanoseconds) || + (info->threshold.min_forward.nanoseconds > 0 && + time_jump->delta.nanoseconds >= info->threshold.min_forward.nanoseconds)) + { + info->callback(time_jump, before_jump, info->user_data); + } + } +} + +rcl_ret_t +rcl_enable_ros_time_override(rcl_clock_t * clock) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + if (clock->type != RCL_ROS_TIME) { + RCL_SET_ERROR_MSG("Clock is not of type RCL_ROS_TIME, cannot enable override."); + return RCL_RET_ERROR; + } + rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data; + RCL_CHECK_FOR_NULL_WITH_MSG( + storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR); + if (!storage->active) { + rcl_time_jump_t time_jump; + time_jump.delta.nanoseconds = 0; + time_jump.clock_change = RCL_ROS_TIME_ACTIVATED; + rcl_clock_call_callbacks(clock, &time_jump, true); + storage->active = true; + rcl_clock_call_callbacks(clock, &time_jump, false); + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_disable_ros_time_override(rcl_clock_t * clock) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + if (clock->type != RCL_ROS_TIME) { + RCL_SET_ERROR_MSG("Clock is not of type RCL_ROS_TIME, cannot disable override."); + return RCL_RET_ERROR; + } + rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data; + RCL_CHECK_FOR_NULL_WITH_MSG( + storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR); + if (storage->active) { + rcl_time_jump_t time_jump; + time_jump.delta.nanoseconds = 0; + time_jump.clock_change = RCL_ROS_TIME_DEACTIVATED; + rcl_clock_call_callbacks(clock, &time_jump, true); + storage->active = false; + rcl_clock_call_callbacks(clock, &time_jump, false); + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_is_enabled_ros_time_override( + rcl_clock_t * clock, + bool * is_enabled) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(is_enabled, RCL_RET_INVALID_ARGUMENT); + if (clock->type != RCL_ROS_TIME) { + RCL_SET_ERROR_MSG("Clock is not of type RCL_ROS_TIME, cannot query override state."); + return RCL_RET_ERROR; + } + rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data; + RCL_CHECK_FOR_NULL_WITH_MSG( + storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR); + *is_enabled = storage->active; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_set_ros_time_override( + rcl_clock_t * clock, + rcl_time_point_value_t time_value) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + if (clock->type != RCL_ROS_TIME) { + RCL_SET_ERROR_MSG("Clock is not of type RCL_ROS_TIME, cannot set time override."); + return RCL_RET_ERROR; + } + rcl_ros_clock_storage_t * storage = (rcl_ros_clock_storage_t *)clock->data; + RCL_CHECK_FOR_NULL_WITH_MSG( + storage, "Clock storage is not initialized, cannot enable override.", return RCL_RET_ERROR); + rcl_time_jump_t time_jump; + if (storage->active) { + time_jump.clock_change = RCL_ROS_TIME_NO_CHANGE; + rcl_time_point_value_t current_time; + rcl_ret_t ret = rcl_get_ros_time(storage, ¤t_time); + if (RCL_RET_OK != ret) { + return ret; + } + time_jump.delta.nanoseconds = time_value - current_time; + rcl_clock_call_callbacks(clock, &time_jump, true); + rcutils_atomic_store(&(storage->current_time), time_value); + rcl_clock_call_callbacks(clock, &time_jump, false); + } else { + rcutils_atomic_store(&(storage->current_time), time_value); + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_clock_add_jump_callback( + rcl_clock_t * clock, rcl_jump_threshold_t threshold, rcl_jump_callback_t callback, + void * user_data) +{ + // Make sure parameters are valid + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG( + &(clock->allocator), "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(callback, RCL_RET_INVALID_ARGUMENT); + if (threshold.min_forward.nanoseconds < 0) { + RCL_SET_ERROR_MSG("forward jump threshold must be positive or zero"); + return RCL_RET_INVALID_ARGUMENT; + } + if (threshold.min_backward.nanoseconds > 0) { + RCL_SET_ERROR_MSG("backward jump threshold must be negative or zero"); + return RCL_RET_INVALID_ARGUMENT; + } + + // Callback/user_data pair must be unique + for (size_t cb_idx = 0; cb_idx < clock->num_jump_callbacks; ++cb_idx) { + const rcl_jump_callback_info_t * info = &(clock->jump_callbacks[cb_idx]); + if (info->callback == callback && info->user_data == user_data) { + RCL_SET_ERROR_MSG("callback/user_data are already added to this clock"); + return RCL_RET_ERROR; + } + } + + // Add the new callback, increasing the size of the callback list + rcl_jump_callback_info_t * callbacks = clock->allocator.reallocate( + clock->jump_callbacks, sizeof(rcl_jump_callback_info_t) * (clock->num_jump_callbacks + 1), + clock->allocator.state); + if (NULL == callbacks) { + RCL_SET_ERROR_MSG("Failed to realloc jump callbacks"); + return RCL_RET_BAD_ALLOC; + } + clock->jump_callbacks = callbacks; + clock->jump_callbacks[clock->num_jump_callbacks].callback = callback; + clock->jump_callbacks[clock->num_jump_callbacks].threshold = threshold; + clock->jump_callbacks[clock->num_jump_callbacks].user_data = user_data; + ++(clock->num_jump_callbacks); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_clock_remove_jump_callback( + rcl_clock_t * clock, rcl_jump_callback_t callback, void * user_data) +{ + // Make sure parameters are valid + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ALLOCATOR_WITH_MSG( + &(clock->allocator), "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(callback, RCL_RET_INVALID_ARGUMENT); + + // Delete callback if found, moving all callbacks after back one + bool found_callback = false; + for (size_t cb_idx = 0; cb_idx < clock->num_jump_callbacks; ++cb_idx) { + const rcl_jump_callback_info_t * info = &(clock->jump_callbacks[cb_idx]); + if (found_callback) { + clock->jump_callbacks[cb_idx - 1] = *info; + } else if (info->callback == callback && info->user_data == user_data) { + found_callback = true; + } + } + if (!found_callback) { + RCL_SET_ERROR_MSG("jump callback was not found"); + return RCL_RET_ERROR; + } + + // Shrink size of the callback array + if (--(clock->num_jump_callbacks) == 0) { + clock->allocator.deallocate(clock->jump_callbacks, clock->allocator.state); + clock->jump_callbacks = NULL; + } else { + rcl_jump_callback_info_t * callbacks = clock->allocator.reallocate( + clock->jump_callbacks, sizeof(rcl_jump_callback_info_t) * clock->num_jump_callbacks, + clock->allocator.state); + if (NULL == callbacks) { + RCL_SET_ERROR_MSG("Failed to shrink jump callbacks"); + return RCL_RET_BAD_ALLOC; + } + clock->jump_callbacks = callbacks; + } + return RCL_RET_OK; +} diff --git a/src/rcl/timer.c b/src/rcl/timer.c new file mode 100644 index 0000000..77239fb --- /dev/null +++ b/src/rcl/timer.c @@ -0,0 +1,457 @@ +// Copyright 2015 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/timer.h" + +#include + +#include "rcl/error_handling.h" +#include "rcutils/logging_macros.h" +#include "rcutils/stdatomic_helper.h" +#include "rcutils/time.h" +#include "tracetools/tracetools.h" + +struct rcl_timer_impl_s +{ + // The clock providing time. + rcl_clock_t * clock; + // The associated context. + rcl_context_t * context; + // A guard condition used to wake the associated wait set, either when + // ROSTime causes the timer to expire or when the timer is reset. + rcl_guard_condition_t guard_condition; + // The user supplied callback. + atomic_uintptr_t callback; + // This is a duration in nanoseconds. + atomic_uint_least64_t period; + // This is a time in nanoseconds since an unspecified time. + atomic_int_least64_t last_call_time; + // This is a time in nanoseconds since an unspecified time. + atomic_int_least64_t next_call_time; + // Credit for time elapsed before ROS time is activated or deactivated. + atomic_int_least64_t time_credit; + // A flag which indicates if the timer is canceled. + atomic_bool canceled; + // The user supplied allocator. + rcl_allocator_t allocator; +}; + +rcl_timer_t +rcl_get_zero_initialized_timer() +{ + static rcl_timer_t null_timer = {0}; + return null_timer; +} + +void _rcl_timer_time_jump( + const rcl_time_jump_t * time_jump, + bool before_jump, + void * user_data) +{ + rcl_timer_t * timer = (rcl_timer_t *)user_data; + + if (before_jump) { + if (RCL_ROS_TIME_ACTIVATED == time_jump->clock_change || + RCL_ROS_TIME_DEACTIVATED == time_jump->clock_change) + { + rcl_time_point_value_t now; + if (RCL_RET_OK != rcl_clock_get_now(timer->impl->clock, &now)) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to get current time in jump callback"); + return; + } + // Source of time is changing, but the timer has ellapsed some portion of its period + // Save ellapsed duration pre jump so the timer only waits the remainder in the new epoch + if (0 == now) { + // No time credit if clock is uninitialized + return; + } + const int64_t next_call_time = rcutils_atomic_load_int64_t(&timer->impl->next_call_time); + rcutils_atomic_store(&timer->impl->time_credit, next_call_time - now); + } + } else { + rcl_time_point_value_t now; + if (RCL_RET_OK != rcl_clock_get_now(timer->impl->clock, &now)) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to get current time in jump callback"); + return; + } + const int64_t last_call_time = rcutils_atomic_load_int64_t(&timer->impl->last_call_time); + const int64_t next_call_time = rcutils_atomic_load_int64_t(&timer->impl->next_call_time); + const int64_t period = rcutils_atomic_load_uint64_t(&timer->impl->period); + if (RCL_ROS_TIME_ACTIVATED == time_jump->clock_change || + RCL_ROS_TIME_DEACTIVATED == time_jump->clock_change) + { + // ROS time activated or deactivated + if (0 == now) { + // Can't apply time credit if clock is uninitialized + return; + } + int64_t time_credit = rcutils_atomic_exchange_int64_t(&timer->impl->time_credit, 0); + if (time_credit) { + // set times in new epoch so timer only waits the remainder of the period + rcutils_atomic_store(&timer->impl->next_call_time, now - time_credit + period); + rcutils_atomic_store(&timer->impl->last_call_time, now - time_credit); + } + } else if (next_call_time <= now) { + // Post Forward jump and timer is ready + if (RCL_RET_OK != rcl_trigger_guard_condition(&timer->impl->guard_condition)) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Failed to get trigger guard condition in jump callback"); + } + } else if (now < last_call_time) { + // Post backwards time jump that went further back than 1 period + // next callback should happen after 1 period + rcutils_atomic_store(&timer->impl->next_call_time, now + period); + rcutils_atomic_store(&timer->impl->last_call_time, now); + return; + } + } +} + +rcl_ret_t +rcl_timer_init( + rcl_timer_t * timer, + rcl_clock_t * clock, + rcl_context_t * context, + int64_t period, + const rcl_timer_callback_t callback, + rcl_allocator_t allocator) +{ + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + if (period < 0) { + RCL_SET_ERROR_MSG("timer period must be non-negative"); + return RCL_RET_INVALID_ARGUMENT; + } + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing timer with period: %" PRIu64 "ns", period); + if (timer->impl) { + RCL_SET_ERROR_MSG("timer already initialized, or memory was uninitialized"); + return RCL_RET_ALREADY_INIT; + } + rcl_time_point_value_t now; + rcl_ret_t now_ret = rcl_clock_get_now(clock, &now); + if (now_ret != RCL_RET_OK) { + return now_ret; // rcl error state should already be set. + } + rcl_timer_impl_t impl; + impl.clock = clock; + impl.context = context; + impl.guard_condition = rcl_get_zero_initialized_guard_condition(); + rcl_guard_condition_options_t options = rcl_guard_condition_get_default_options(); + rcl_ret_t ret = rcl_guard_condition_init(&(impl.guard_condition), context, options); + if (RCL_RET_OK != ret) { + return ret; + } + if (RCL_ROS_TIME == impl.clock->type) { + rcl_jump_threshold_t threshold; + threshold.on_clock_change = true; + threshold.min_forward.nanoseconds = 1; + threshold.min_backward.nanoseconds = -1; + ret = rcl_clock_add_jump_callback(clock, threshold, _rcl_timer_time_jump, timer); + if (RCL_RET_OK != ret) { + if (RCL_RET_OK != rcl_guard_condition_fini(&(impl.guard_condition))) { + // Should be impossible + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Failed to fini guard condition after failing to add jump callback"); + } + return ret; + } + } + atomic_init(&impl.callback, (uintptr_t)callback); + atomic_init(&impl.period, period); + atomic_init(&impl.time_credit, 0); + atomic_init(&impl.last_call_time, now); + atomic_init(&impl.next_call_time, now + period); + atomic_init(&impl.canceled, false); + impl.allocator = allocator; + timer->impl = (rcl_timer_impl_t *)allocator.allocate(sizeof(rcl_timer_impl_t), allocator.state); + if (NULL == timer->impl) { + if (RCL_RET_OK != rcl_guard_condition_fini(&(impl.guard_condition))) { + // Should be impossible + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to fini guard condition after bad alloc"); + } + if (RCL_RET_OK != rcl_clock_remove_jump_callback(clock, _rcl_timer_time_jump, timer)) { + // Should be impossible + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to remove callback after bad alloc"); + } + + RCL_SET_ERROR_MSG("allocating memory failed"); + return RCL_RET_BAD_ALLOC; + } + *timer->impl = impl; + TRACEPOINT(rcl_timer_init, (const void *)timer, period); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_fini(rcl_timer_t * timer) +{ + if (!timer || !timer->impl) { + return RCL_RET_OK; + } + // Will return either RCL_RET_OK or RCL_RET_ERROR since the timer is valid. + rcl_ret_t result = rcl_timer_cancel(timer); + rcl_allocator_t allocator = timer->impl->allocator; + rcl_ret_t fail_ret; + if (RCL_ROS_TIME == timer->impl->clock->type) { + // The jump callbacks use the guard condition, so we have to remove it + // before freeing the guard condition below. + fail_ret = rcl_clock_remove_jump_callback(timer->impl->clock, _rcl_timer_time_jump, timer); + if (RCL_RET_OK != fail_ret) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to remove timer jump callback"); + } + } + fail_ret = rcl_guard_condition_fini(&(timer->impl->guard_condition)); + if (RCL_RET_OK != fail_ret) { + RCL_SET_ERROR_MSG("Failure to fini guard condition"); + } + allocator.deallocate(timer->impl, allocator.state); + timer->impl = NULL; + return result; +} + +RCL_PUBLIC +RCL_WARN_UNUSED +rcl_ret_t +rcl_timer_clock(rcl_timer_t * timer, rcl_clock_t ** clock) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(clock, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID); + *clock = timer->impl->clock; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_call(rcl_timer_t * timer) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Calling timer"); + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID); + if (rcutils_atomic_load_bool(&timer->impl->canceled)) { + RCL_SET_ERROR_MSG("timer is canceled"); + return RCL_RET_TIMER_CANCELED; + } + rcl_time_point_value_t now; + rcl_ret_t now_ret = rcl_clock_get_now(timer->impl->clock, &now); + if (now_ret != RCL_RET_OK) { + return now_ret; // rcl error state should already be set. + } + if (now < 0) { + RCL_SET_ERROR_MSG("clock now returned negative time point value"); + return RCL_RET_ERROR; + } + rcl_time_point_value_t previous_ns = + rcutils_atomic_exchange_int64_t(&timer->impl->last_call_time, now); + rcl_timer_callback_t typed_callback = + (rcl_timer_callback_t)rcutils_atomic_load_uintptr_t(&timer->impl->callback); + + int64_t next_call_time = rcutils_atomic_load_int64_t(&timer->impl->next_call_time); + int64_t period = rcutils_atomic_load_uint64_t(&timer->impl->period); + // always move the next call time by exactly period forward + // don't use now as the base to avoid extending each cycle by the time + // between the timer being ready and the callback being triggered + next_call_time += period; + // in case the timer has missed at least once cycle + if (next_call_time < now) { + if (0 == period) { + // a timer with a period of zero is considered always ready + next_call_time = now; + } else { + // move the next call time forward by as many periods as necessary + int64_t now_ahead = now - next_call_time; + // rounding up without overflow + int64_t periods_ahead = 1 + (now_ahead - 1) / period; + next_call_time += periods_ahead * period; + } + } + rcutils_atomic_store(&timer->impl->next_call_time, next_call_time); + + if (typed_callback != NULL) { + int64_t since_last_call = now - previous_ns; + typed_callback(timer, since_last_call); + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_is_ready(const rcl_timer_t * timer, bool * is_ready) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(is_ready, RCL_RET_INVALID_ARGUMENT); + int64_t time_until_next_call; + rcl_ret_t ret = rcl_timer_get_time_until_next_call(timer, &time_until_next_call); + if (ret == RCL_RET_TIMER_CANCELED) { + *is_ready = false; + return RCL_RET_OK; + } else if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } + *is_ready = (time_until_next_call <= 0); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_get_time_until_next_call(const rcl_timer_t * timer, int64_t * time_until_next_call) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(time_until_next_call, RCL_RET_INVALID_ARGUMENT); + if (rcutils_atomic_load_bool(&timer->impl->canceled)) { + return RCL_RET_TIMER_CANCELED; + } + rcl_time_point_value_t now; + rcl_ret_t ret = rcl_clock_get_now(timer->impl->clock, &now); + if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } + *time_until_next_call = + rcutils_atomic_load_int64_t(&timer->impl->next_call_time) - now; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_get_time_since_last_call( + const rcl_timer_t * timer, + rcl_time_point_value_t * time_since_last_call) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(time_since_last_call, RCL_RET_INVALID_ARGUMENT); + rcl_time_point_value_t now; + rcl_ret_t ret = rcl_clock_get_now(timer->impl->clock, &now); + if (ret != RCL_RET_OK) { + return ret; // rcl error state should already be set. + } + *time_since_last_call = + now - rcutils_atomic_load_int64_t(&timer->impl->last_call_time); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_get_period(const rcl_timer_t * timer, int64_t * period) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(period, RCL_RET_INVALID_ARGUMENT); + *period = rcutils_atomic_load_uint64_t(&timer->impl->period); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_exchange_period(const rcl_timer_t * timer, int64_t new_period, int64_t * old_period) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(old_period, RCL_RET_INVALID_ARGUMENT); + *old_period = rcutils_atomic_exchange_uint64_t(&timer->impl->period, new_period); + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Updated timer period from '%" PRIu64 "ns' to '%" PRIu64 "ns'", + *old_period, new_period); + return RCL_RET_OK; +} + +rcl_timer_callback_t +rcl_timer_get_callback(const rcl_timer_t * timer) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL); + return (rcl_timer_callback_t)rcutils_atomic_load_uintptr_t(&timer->impl->callback); +} + +rcl_timer_callback_t +rcl_timer_exchange_callback(rcl_timer_t * timer, const rcl_timer_callback_t new_callback) +{ + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Updating timer callback"); + RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL); + return (rcl_timer_callback_t)rcutils_atomic_exchange_uintptr_t( + &timer->impl->callback, (uintptr_t)new_callback); +} + +rcl_ret_t +rcl_timer_cancel(rcl_timer_t * timer) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_TIMER_INVALID); + + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + rcutils_atomic_store(&timer->impl->canceled, true); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer canceled"); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_is_canceled(const rcl_timer_t * timer, bool * is_canceled) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(timer->impl, RCL_RET_TIMER_INVALID); + RCL_CHECK_ARGUMENT_FOR_NULL(is_canceled, RCL_RET_INVALID_ARGUMENT); + *is_canceled = rcutils_atomic_load_bool(&timer->impl->canceled); + return RCL_RET_OK; +} + +rcl_ret_t +rcl_timer_reset(rcl_timer_t * timer) +{ + RCUTILS_CAN_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + + RCL_CHECK_ARGUMENT_FOR_NULL(timer, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return RCL_RET_TIMER_INVALID); + rcl_time_point_value_t now; + rcl_ret_t now_ret = rcl_clock_get_now(timer->impl->clock, &now); + if (now_ret != RCL_RET_OK) { + return now_ret; // rcl error state should already be set. + } + int64_t period = rcutils_atomic_load_uint64_t(&timer->impl->period); + rcutils_atomic_store(&timer->impl->next_call_time, now + period); + rcutils_atomic_store(&timer->impl->canceled, false); + rcl_ret_t ret = rcl_trigger_guard_condition(&timer->impl->guard_condition); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Failed to trigger timer guard condition"); + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Timer successfully reset"); + return RCL_RET_OK; +} + +const rcl_allocator_t * +rcl_timer_get_allocator(const rcl_timer_t * timer) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(timer, NULL); + RCL_CHECK_FOR_NULL_WITH_MSG(timer->impl, "timer is invalid", return NULL); + return &timer->impl->allocator; +} + +rcl_guard_condition_t * +rcl_timer_get_guard_condition(const rcl_timer_t * timer) +{ + if (NULL == timer || NULL == timer->impl || NULL == timer->impl->guard_condition.impl) { + return NULL; + } + return &timer->impl->guard_condition; +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/validate_enclave_name.c b/src/rcl/validate_enclave_name.c new file mode 100644 index 0000000..8cd421c --- /dev/null +++ b/src/rcl/validate_enclave_name.c @@ -0,0 +1,151 @@ +// 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 "rcl/validate_enclave_name.h" + +#include +#include +#include + +#include +#include + +#include "rmw/validate_namespace.h" + +#include "rcl/error_handling.h" + +#include "./common.h" + +rcl_ret_t +rcl_validate_enclave_name( + const char * enclave, + int * validation_result, + size_t * invalid_index) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(enclave, RCL_RET_INVALID_ARGUMENT); + return rcl_validate_enclave_name_with_size( + enclave, strlen(enclave), validation_result, invalid_index); +} + +rcl_ret_t +rcl_validate_enclave_name_with_size( + const char * enclave, + size_t enclave_length, + int * validation_result, + size_t * invalid_index) +{ + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT); + RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR); + + RCL_CHECK_ARGUMENT_FOR_NULL(enclave, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(validation_result, RCL_RET_INVALID_ARGUMENT); + + int tmp_validation_result; + size_t tmp_invalid_index; + rmw_ret_t ret = rmw_validate_namespace_with_size( + enclave, enclave_length, &tmp_validation_result, &tmp_invalid_index); + if (ret != RMW_RET_OK) { + return rcl_convert_rmw_ret_to_rcl_ret(ret); + } + + if (tmp_validation_result != RMW_NAMESPACE_VALID && + tmp_validation_result != RMW_NAMESPACE_INVALID_TOO_LONG) + { + switch (tmp_validation_result) { + case RMW_NAMESPACE_INVALID_IS_EMPTY_STRING: + *validation_result = RCL_ENCLAVE_NAME_INVALID_IS_EMPTY_STRING; + break; + case RMW_NAMESPACE_INVALID_NOT_ABSOLUTE: + *validation_result = RCL_ENCLAVE_NAME_INVALID_NOT_ABSOLUTE; + break; + case RMW_NAMESPACE_INVALID_ENDS_WITH_FORWARD_SLASH: + *validation_result = RCL_ENCLAVE_NAME_INVALID_ENDS_WITH_FORWARD_SLASH; + break; + case RMW_NAMESPACE_INVALID_CONTAINS_UNALLOWED_CHARACTERS: + *validation_result = RCL_ENCLAVE_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS; + break; + case RMW_NAMESPACE_INVALID_CONTAINS_REPEATED_FORWARD_SLASH: + *validation_result = RCL_ENCLAVE_NAME_INVALID_CONTAINS_REPEATED_FORWARD_SLASH; + break; + case RMW_NAMESPACE_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER: + *validation_result = RCL_ENCLAVE_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER; + break; + default: + { + char default_err_msg[256]; + // explicitly not taking return value which is number of bytes written + int ret = rcutils_snprintf( + default_err_msg, sizeof(default_err_msg), + "rcl_validate_enclave_name_with_size(): " + "unknown rmw_validate_namespace_with_size() result '%d'", + tmp_validation_result); + if (ret < 0) { + RCL_SET_ERROR_MSG( + "rcl_validate_enclave_name_with_size(): " + "rcutils_snprintf() failed while reporting an unknown validation result"); + } else { + RCL_SET_ERROR_MSG(default_err_msg); + } + } + return RCL_RET_ERROR; + } + if (invalid_index) { + *invalid_index = tmp_invalid_index; + } + return RCL_RET_OK; + } + + // enclave might be longer that namespace length, check false positives and correct + if (RMW_NAMESPACE_INVALID_TOO_LONG == tmp_validation_result) { + if (RCL_ENCLAVE_NAME_MAX_LENGTH >= enclave_length) { + *validation_result = RCL_ENCLAVE_NAME_VALID; + } else { + *validation_result = RCL_ENCLAVE_NAME_INVALID_TOO_LONG; + if (invalid_index) { + *invalid_index = RCL_ENCLAVE_NAME_MAX_LENGTH - 1; + } + } + return RCL_RET_OK; + } + + // everything was ok, set result to valid namespace, avoid setting invalid_index, and return + *validation_result = RCL_ENCLAVE_NAME_VALID; + return RCL_RET_OK; +} + +const char * +rcl_enclave_name_validation_result_string(int validation_result) +{ + switch (validation_result) { + case RCL_ENCLAVE_NAME_VALID: + return NULL; + case RCL_ENCLAVE_NAME_INVALID_IS_EMPTY_STRING: + return "context name must not be empty"; + case RCL_ENCLAVE_NAME_INVALID_NOT_ABSOLUTE: + return "context name must be absolute, it must lead with a '/'"; + case RCL_ENCLAVE_NAME_INVALID_ENDS_WITH_FORWARD_SLASH: + return "context name must not end with a '/', unless only a '/'"; + case RCL_ENCLAVE_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS: + return "context name must not contain characters other than alphanumerics, '_', or '/'"; + case RCL_ENCLAVE_NAME_INVALID_CONTAINS_REPEATED_FORWARD_SLASH: + return "context name must not contain repeated '/'"; + case RCL_ENCLAVE_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER: + return "context name must not have a token that starts with a number"; + case RCL_ENCLAVE_NAME_INVALID_TOO_LONG: + return "context name should not exceed '" + RCUTILS_STRINGIFY(RCL_ENCLAVE_NAME_MAX_NAME_LENGTH) "'"; + default: + return "unknown result code for rcl context name validation"; + } +} diff --git a/src/rcl/validate_topic_name.c b/src/rcl/validate_topic_name.c new file mode 100644 index 0000000..671ae38 --- /dev/null +++ b/src/rcl/validate_topic_name.c @@ -0,0 +1,234 @@ +// 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/validate_topic_name.h" + +#include +#include + +#include "rcl/allocator.h" +#include "rcl/error_handling.h" +#include "rcutils/isalnum_no_locale.h" + +rcl_ret_t +rcl_validate_topic_name( + const char * topic_name, + int * validation_result, + size_t * invalid_index) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + return rcl_validate_topic_name_with_size( + topic_name, strlen(topic_name), validation_result, invalid_index); +} + +rcl_ret_t +rcl_validate_topic_name_with_size( + const char * topic_name, + size_t topic_name_length, + int * validation_result, + size_t * invalid_index) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(topic_name, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(validation_result, RCL_RET_INVALID_ARGUMENT); + + if (topic_name_length == 0) { + *validation_result = RCL_TOPIC_NAME_INVALID_IS_EMPTY_STRING; + if (invalid_index) { + *invalid_index = 0; + } + return RCL_RET_OK; + } + // check that the first character is not a number + if (isdigit(topic_name[0]) != 0) { + // this is the case where the topic is relative and the first token starts with a number + // e.g. 7foo/bar is invalid + *validation_result = RCL_TOPIC_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER; + if (invalid_index) { + *invalid_index = 0; + } + return RCL_RET_OK; + } + // note topic_name_length is >= 1 at this point + if (topic_name[topic_name_length - 1] == '/') { + // catches both "/foo/" and "/" + *validation_result = RCL_TOPIC_NAME_INVALID_ENDS_WITH_FORWARD_SLASH; + if (invalid_index) { + *invalid_index = topic_name_length - 1; + } + return RCL_RET_OK; + } + // check for unallowed characters, nested and unmatched {} too + bool in_open_curly_brace = false; + size_t opening_curly_brace_index = 0; + for (size_t i = 0; i < topic_name_length; ++i) { + if (rcutils_isalnum_no_locale(topic_name[i])) { + // if within curly braces and the first character is a number, error + // e.g. foo/{4bar} is invalid + if ( + isdigit(topic_name[i]) != 0 && + in_open_curly_brace && + i > 0 && + (i - 1 == opening_curly_brace_index)) + { + *validation_result = RCL_TOPIC_NAME_INVALID_SUBSTITUTION_STARTS_WITH_NUMBER; + if (invalid_index) { + *invalid_index = i; + } + return RCL_RET_OK; + } + // if it is an alpha numeric character, i.e. [0-9|A-Z|a-z], continue + continue; + } else if (topic_name[i] == '_') { + // if it is an underscore, continue + continue; + } else if (topic_name[i] == '/') { + // if it is a forward slash within {}, error + if (in_open_curly_brace) { + *validation_result = RCL_TOPIC_NAME_INVALID_SUBSTITUTION_CONTAINS_UNALLOWED_CHARACTERS; + if (invalid_index) { + *invalid_index = i; + } + return RCL_RET_OK; + } + // if it is a forward slash outside of {}, continue + continue; + } else if (topic_name[i] == '~') { + // if it is a tilde not in the first position, validation fails + if (i != 0) { + *validation_result = RCL_TOPIC_NAME_INVALID_MISPLACED_TILDE; + if (invalid_index) { + *invalid_index = i; + } + return RCL_RET_OK; + } + // if it is a tilde in the first position, continue + continue; + } else if (topic_name[i] == '{') { + opening_curly_brace_index = i; + // if starting a nested curly brace, error + // e.g. foo/{{bar}_baz} is invalid + // ^ + if (in_open_curly_brace) { + *validation_result = RCL_TOPIC_NAME_INVALID_SUBSTITUTION_CONTAINS_UNALLOWED_CHARACTERS; + if (invalid_index) { + *invalid_index = i; + } + return RCL_RET_OK; + } + in_open_curly_brace = true; + // if it is a new, open curly brace, continue + continue; + } else if (topic_name[i] == '}') { + // if not preceded by a {, error + if (!in_open_curly_brace) { + *validation_result = RCL_TOPIC_NAME_INVALID_UNMATCHED_CURLY_BRACE; + if (invalid_index) { + *invalid_index = i; + } + return RCL_RET_OK; + } + in_open_curly_brace = false; + // if it is a closing curly brace, continue + continue; + } else { + // if it is none of these, then it is an unallowed character in a topic name + if (in_open_curly_brace) { + *validation_result = RCL_TOPIC_NAME_INVALID_SUBSTITUTION_CONTAINS_UNALLOWED_CHARACTERS; + } else { + *validation_result = RCL_TOPIC_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS; + } + if (invalid_index) { + *invalid_index = i; + } + return RCL_RET_OK; + } + } + // check to make sure substitutions were properly closed + if (in_open_curly_brace) { + // case where a substitution is never closed, e.g. 'foo/{bar' + *validation_result = RCL_TOPIC_NAME_INVALID_UNMATCHED_CURLY_BRACE; + if (invalid_index) { + *invalid_index = opening_curly_brace_index; + } + return RCL_RET_OK; + } + // check for tokens (other than the first) that start with a number + for (size_t i = 0; i < topic_name_length; ++i) { + if (i == topic_name_length - 1) { + // if this is the last character, then nothing to check + continue; + } + // past this point, assuming i+1 is a valid index + if (topic_name[i] == '/') { + if (isdigit(topic_name[i + 1]) != 0) { + // this is the case where a '/' if followed by a number, i.e. [0-9] + *validation_result = RCL_TOPIC_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER; + if (invalid_index) { + *invalid_index = i + 1; + } + return RCL_RET_OK; + } + } else if (i == 1 && topic_name[0] == '~') { + // special case where first character is ~ but second character is not / + // e.g. ~foo is invalid + *validation_result = RCL_TOPIC_NAME_INVALID_TILDE_NOT_FOLLOWED_BY_FORWARD_SLASH; + if (invalid_index) { + *invalid_index = 1; + } + return RCL_RET_OK; + } + } + // everything was ok, set result to valid topic, avoid setting invalid_index, and return + *validation_result = RCL_TOPIC_NAME_VALID; + return RCL_RET_OK; +} + +const char * +rcl_topic_name_validation_result_string(int validation_result) +{ + switch (validation_result) { + case RCL_TOPIC_NAME_VALID: + return NULL; + case RCL_TOPIC_NAME_INVALID_IS_EMPTY_STRING: + return "topic name must not be empty string"; + case RCL_TOPIC_NAME_INVALID_ENDS_WITH_FORWARD_SLASH: + return "topic name must not end with a forward slash"; + case RCL_TOPIC_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS: + return + "topic name must not contain characters other than alphanumerics, '_', '~', '{', or '}'"; + case RCL_TOPIC_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER: + return "topic name token must not start with a number"; + case RCL_TOPIC_NAME_INVALID_UNMATCHED_CURLY_BRACE: + return "topic name must not have unmatched (unbalanced) curly braces '{}'"; + case RCL_TOPIC_NAME_INVALID_MISPLACED_TILDE: + return "topic name must not have tilde '~' unless it is the first character"; + case RCL_TOPIC_NAME_INVALID_TILDE_NOT_FOLLOWED_BY_FORWARD_SLASH: + return "topic name must not have a tilde '~' that is not followed by a forward slash '/'"; + case RCL_TOPIC_NAME_INVALID_SUBSTITUTION_CONTAINS_UNALLOWED_CHARACTERS: + return "substitution name must not contain characters other than alphanumerics or '_'"; + case RCL_TOPIC_NAME_INVALID_SUBSTITUTION_STARTS_WITH_NUMBER: + return "substitution name must not start with a number"; + default: + return "unknown result code for rcl topic name validation"; + } +} + +#ifdef __cplusplus +} +#endif diff --git a/src/rcl/wait.c b/src/rcl/wait.c new file mode 100644 index 0000000..7bcdedb --- /dev/null +++ b/src/rcl/wait.c @@ -0,0 +1,677 @@ +// Copyright 2015 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. + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include "rcl/wait.h" + +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/time.h" +#include "rcutils/logging_macros.h" +#include "rmw/error_handling.h" +#include "rmw/rmw.h" +#include "rmw/event.h" + +#include "./context_impl.h" + +struct rcl_wait_set_impl_s +{ + // number of subscriptions that have been added to the wait set + size_t subscription_index; + rmw_subscriptions_t rmw_subscriptions; + // number of guard_conditions that have been added to the wait set + size_t guard_condition_index; + rmw_guard_conditions_t rmw_guard_conditions; + // number of clients that have been added to the wait set + size_t client_index; + rmw_clients_t rmw_clients; + // number of services that have been added to the wait set + size_t service_index; + rmw_services_t rmw_services; + // number of events that have been added to the wait set + size_t event_index; + rmw_events_t rmw_events; + + rmw_wait_set_t * rmw_wait_set; + // number of timers that have been added to the wait set + size_t timer_index; + // context with which the wait set is associated + rcl_context_t * context; + // allocator used in the wait set + rcl_allocator_t allocator; +}; + +rcl_wait_set_t +rcl_get_zero_initialized_wait_set() +{ + static rcl_wait_set_t null_wait_set = { + .subscriptions = NULL, + .size_of_subscriptions = 0, + .guard_conditions = NULL, + .size_of_guard_conditions = 0, + .clients = NULL, + .size_of_clients = 0, + .services = NULL, + .size_of_services = 0, + .timers = NULL, + .size_of_timers = 0, + .events = NULL, + .size_of_events = 0, + .impl = NULL, + }; + return null_wait_set; +} + +bool +rcl_wait_set_is_valid(const rcl_wait_set_t * wait_set) +{ + return wait_set && wait_set->impl; +} + +static void +__wait_set_clean_up(rcl_wait_set_t * wait_set) +{ + rcl_ret_t ret = rcl_wait_set_resize(wait_set, 0, 0, 0, 0, 0, 0); + (void)ret; // NO LINT + assert(RCL_RET_OK == ret); // Defensive, shouldn't fail with size 0. + if (wait_set->impl) { + wait_set->impl->allocator.deallocate(wait_set->impl, wait_set->impl->allocator.state); + wait_set->impl = NULL; + } +} + +rcl_ret_t +rcl_wait_set_init( + rcl_wait_set_t * wait_set, + size_t number_of_subscriptions, + size_t number_of_guard_conditions, + size_t number_of_timers, + size_t number_of_clients, + size_t number_of_services, + size_t number_of_events, + rcl_context_t * context, + rcl_allocator_t allocator) +{ + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Initializing wait set with " + "'%zu' subscriptions, '%zu' guard conditions, '%zu' timers, '%zu' clients, '%zu' services", + number_of_subscriptions, number_of_guard_conditions, number_of_timers, number_of_clients, + number_of_services); + rcl_ret_t fail_ret = RCL_RET_ERROR; + + RCL_CHECK_ALLOCATOR_WITH_MSG(&allocator, "invalid allocator", return RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); + if (rcl_wait_set_is_valid(wait_set)) { + RCL_SET_ERROR_MSG("wait_set already initialized, or memory was uninitialized."); + return RCL_RET_ALREADY_INIT; + } + // Make sure rcl has been initialized. + RCL_CHECK_ARGUMENT_FOR_NULL(context, RCL_RET_INVALID_ARGUMENT); + if (!rcl_context_is_valid(context)) { + RCL_SET_ERROR_MSG( + "the given context is not valid, " + "either rcl_init() was not called or rcl_shutdown() was called."); + return RCL_RET_NOT_INIT; + } + // Allocate space for the implementation struct. + wait_set->impl = (rcl_wait_set_impl_t *)allocator.allocate( + sizeof(rcl_wait_set_impl_t), allocator.state); + RCL_CHECK_FOR_NULL_WITH_MSG( + wait_set->impl, "allocating memory failed", return RCL_RET_BAD_ALLOC); + memset(wait_set->impl, 0, sizeof(rcl_wait_set_impl_t)); + wait_set->impl->rmw_subscriptions.subscribers = NULL; + wait_set->impl->rmw_subscriptions.subscriber_count = 0; + wait_set->impl->rmw_guard_conditions.guard_conditions = NULL; + wait_set->impl->rmw_guard_conditions.guard_condition_count = 0; + wait_set->impl->rmw_clients.clients = NULL; + wait_set->impl->rmw_clients.client_count = 0; + wait_set->impl->rmw_services.services = NULL; + wait_set->impl->rmw_services.service_count = 0; + wait_set->impl->rmw_events.events = NULL; + wait_set->impl->rmw_events.event_count = 0; + // Set context. + wait_set->impl->context = context; + // Set allocator. + wait_set->impl->allocator = allocator; + + size_t num_conditions = + (2 * number_of_subscriptions) + + number_of_guard_conditions + + number_of_clients + + number_of_services + + number_of_events; + + wait_set->impl->rmw_wait_set = rmw_create_wait_set(&(context->impl->rmw_context), num_conditions); + if (!wait_set->impl->rmw_wait_set) { + goto fail; + } + + // Initialize subscription space. + rcl_ret_t ret = rcl_wait_set_resize( + wait_set, number_of_subscriptions, number_of_guard_conditions, number_of_timers, + number_of_clients, number_of_services, number_of_events); + if (RCL_RET_OK != ret) { + fail_ret = ret; + goto fail; + } + return RCL_RET_OK; +fail: + if (rcl_wait_set_is_valid(wait_set)) { + rmw_ret_t ret = rmw_destroy_wait_set(wait_set->impl->rmw_wait_set); + if (ret != RMW_RET_OK) { + fail_ret = RCL_RET_WAIT_SET_INVALID; + } + } + __wait_set_clean_up(wait_set); + return fail_ret; +} + +rcl_ret_t +rcl_wait_set_fini(rcl_wait_set_t * wait_set) +{ + rcl_ret_t result = RCL_RET_OK; + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); + + if (rcl_wait_set_is_valid(wait_set)) { + rmw_ret_t ret = rmw_destroy_wait_set(wait_set->impl->rmw_wait_set); + if (ret != RMW_RET_OK) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + result = RCL_RET_WAIT_SET_INVALID; + } + __wait_set_clean_up(wait_set); + } + return result; +} + +rcl_ret_t +rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * allocator) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); + if (!rcl_wait_set_is_valid(wait_set)) { + RCL_SET_ERROR_MSG("wait set is invalid"); + return RCL_RET_WAIT_SET_INVALID; + } + RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT); + *allocator = wait_set->impl->allocator; + return RCL_RET_OK; +} + +#define SET_ADD(Type) \ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); \ + if (!wait_set->impl) { \ + RCL_SET_ERROR_MSG("wait set is invalid"); \ + return RCL_RET_WAIT_SET_INVALID; \ + } \ + RCL_CHECK_ARGUMENT_FOR_NULL(Type, RCL_RET_INVALID_ARGUMENT); \ + if (!(wait_set->impl->Type ## _index < wait_set->size_of_ ## Type ## s)) { \ + RCL_SET_ERROR_MSG(#Type "s set is full"); \ + return RCL_RET_WAIT_SET_FULL; \ + } \ + size_t current_index = wait_set->impl->Type ## _index++; \ + wait_set->Type ## s[current_index] = Type; \ + /* Set optional output argument */ \ + if (NULL != index) { \ + *index = current_index; \ + } + +#define SET_ADD_RMW(Type, RMWStorage, RMWCount) \ + /* Also place into rmw storage. */ \ + rmw_ ## Type ## _t * rmw_handle = rcl_ ## Type ## _get_rmw_handle(Type); \ + RCL_CHECK_FOR_NULL_WITH_MSG( \ + rmw_handle, rcl_get_error_string().str, return RCL_RET_ERROR); \ + wait_set->impl->RMWStorage[current_index] = rmw_handle->data; \ + wait_set->impl->RMWCount++; + +#define SET_CLEAR(Type) \ + do { \ + if (NULL != wait_set->Type ## s) { \ + memset( \ + (void *)wait_set->Type ## s, \ + 0, \ + sizeof(rcl_ ## Type ## _t *) * wait_set->size_of_ ## Type ## s); \ + wait_set->impl->Type ## _index = 0; \ + } \ + } while (false) + +#define SET_CLEAR_RMW(Type, RMWStorage, RMWCount) \ + do { \ + if (NULL != wait_set->impl->RMWStorage) { \ + /* Also clear the rmw storage. */ \ + memset( \ + wait_set->impl->RMWStorage, \ + 0, \ + sizeof(void *) * wait_set->impl->RMWCount); \ + wait_set->impl->RMWCount = 0; \ + } \ + } while (false) + +#define SET_RESIZE(Type, ExtraDealloc, ExtraRealloc) \ + do { \ + rcl_allocator_t allocator = wait_set->impl->allocator; \ + wait_set->size_of_ ## Type ## s = 0; \ + wait_set->impl->Type ## _index = 0; \ + if (0 == Type ## s_size) { \ + if (wait_set->Type ## s) { \ + allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ + wait_set->Type ## s = NULL; \ + } \ + ExtraDealloc \ + } else { \ + wait_set->Type ## s = (const rcl_ ## Type ## _t **)allocator.reallocate( \ + (void *)wait_set->Type ## s, sizeof(rcl_ ## Type ## _t *) * Type ## s_size, \ + allocator.state); \ + RCL_CHECK_FOR_NULL_WITH_MSG( \ + wait_set->Type ## s, "allocating memory failed", return RCL_RET_BAD_ALLOC); \ + memset((void *)wait_set->Type ## s, 0, sizeof(rcl_ ## Type ## _t *) * Type ## s_size); \ + wait_set->size_of_ ## Type ## s = Type ## s_size; \ + ExtraRealloc \ + } \ + } while (false) + +#define SET_RESIZE_RMW_DEALLOC(RMWStorage, RMWCount) \ + /* Also deallocate the rmw storage. */ \ + if (wait_set->impl->RMWStorage) { \ + allocator.deallocate((void *)wait_set->impl->RMWStorage, allocator.state); \ + wait_set->impl->RMWStorage = NULL; \ + wait_set->impl->RMWCount = 0; \ + } + +#define SET_RESIZE_RMW_REALLOC(Type, RMWStorage, RMWCount) \ + /* Also resize the rmw storage. */ \ + wait_set->impl->RMWCount = 0; \ + wait_set->impl->RMWStorage = (void **)allocator.reallocate( \ + wait_set->impl->RMWStorage, sizeof(void *) * Type ## s_size, allocator.state); \ + if (!wait_set->impl->RMWStorage) { \ + allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ + wait_set->Type ## s = NULL; \ + wait_set->size_of_ ## Type ## s = 0; \ + RCL_SET_ERROR_MSG("allocating memory failed"); \ + return RCL_RET_BAD_ALLOC; \ + } \ + memset(wait_set->impl->RMWStorage, 0, sizeof(void *) * Type ## s_size); + +/* Implementation-specific notes: + * + * Add the rmw representation to the underlying rmw array and increment + * the rmw array count. + */ +rcl_ret_t +rcl_wait_set_add_subscription( + rcl_wait_set_t * wait_set, + const rcl_subscription_t * subscription, + size_t * index) +{ + SET_ADD(subscription) + SET_ADD_RMW(subscription, rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count) + return RCL_RET_OK; +} + +/* Implementation-specific notes: + * + * Sets all of the entries in the underlying rmw array to null, and sets the + * count in the rmw array to 0. + */ +rcl_ret_t +rcl_wait_set_clear(rcl_wait_set_t * wait_set) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set->impl, RCL_RET_WAIT_SET_INVALID); + + SET_CLEAR(subscription); + SET_CLEAR(guard_condition); + SET_CLEAR(client); + SET_CLEAR(service); + SET_CLEAR(event); + SET_CLEAR(timer); + + SET_CLEAR_RMW( + subscription, + rmw_subscriptions.subscribers, + rmw_subscriptions.subscriber_count); + SET_CLEAR_RMW( + guard_condition, + rmw_guard_conditions.guard_conditions, + rmw_guard_conditions.guard_condition_count); + SET_CLEAR_RMW( + clients, + rmw_clients.clients, + rmw_clients.client_count); + SET_CLEAR_RMW( + services, + rmw_services.services, + rmw_services.service_count); + SET_CLEAR_RMW( + events, + rmw_events.events, + rmw_events.event_count); + + return RCL_RET_OK; +} + +/* Implementation-specific notes: + * + * Similarly, the underlying rmw representation is reallocated and reset: + * all entries are set to null and the count is set to zero. + */ +rcl_ret_t +rcl_wait_set_resize( + rcl_wait_set_t * wait_set, + size_t subscriptions_size, + size_t guard_conditions_size, + size_t timers_size, + size_t clients_size, + size_t services_size, + size_t events_size) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set->impl, RCL_RET_WAIT_SET_INVALID); + SET_RESIZE( + subscription, + SET_RESIZE_RMW_DEALLOC( + rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count), + SET_RESIZE_RMW_REALLOC( + subscription, rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count) + ); + // Guard condition RCL size is the resize amount given + SET_RESIZE(guard_condition,;,;); // NOLINT + + // Guard condition RMW size needs to be guard conditions + timers + rmw_guard_conditions_t * rmw_gcs = &(wait_set->impl->rmw_guard_conditions); + const size_t num_rmw_gc = guard_conditions_size + timers_size; + // Clear added guard conditions + rmw_gcs->guard_condition_count = 0u; + if (0u == num_rmw_gc) { + if (rmw_gcs->guard_conditions) { + wait_set->impl->allocator.deallocate( + (void *)rmw_gcs->guard_conditions, wait_set->impl->allocator.state); + rmw_gcs->guard_conditions = NULL; + } + } else { + rmw_gcs->guard_conditions = (void **)wait_set->impl->allocator.reallocate( + rmw_gcs->guard_conditions, sizeof(void *) * num_rmw_gc, wait_set->impl->allocator.state); + if (!rmw_gcs->guard_conditions) { + // Deallocate rcl arrays to match unallocated rmw guard conditions + wait_set->impl->allocator.deallocate( + (void *)wait_set->guard_conditions, wait_set->impl->allocator.state); + wait_set->size_of_guard_conditions = 0u; + wait_set->guard_conditions = NULL; + wait_set->impl->allocator.deallocate( + (void *)wait_set->timers, wait_set->impl->allocator.state); + wait_set->size_of_timers = 0u; + wait_set->timers = NULL; + RCL_SET_ERROR_MSG("allocating memory failed"); + return RCL_RET_BAD_ALLOC; + } + memset(rmw_gcs->guard_conditions, 0, sizeof(void *) * num_rmw_gc); + } + + SET_RESIZE(timer,;,;); // NOLINT + SET_RESIZE( + client, + SET_RESIZE_RMW_DEALLOC( + rmw_clients.clients, rmw_clients.client_count), + SET_RESIZE_RMW_REALLOC( + client, rmw_clients.clients, rmw_clients.client_count) + ); + SET_RESIZE( + service, + SET_RESIZE_RMW_DEALLOC( + rmw_services.services, rmw_services.service_count), + SET_RESIZE_RMW_REALLOC( + service, rmw_services.services, rmw_services.service_count) + ); + SET_RESIZE( + event, + SET_RESIZE_RMW_DEALLOC( + rmw_events.events, rmw_events.event_count), + SET_RESIZE_RMW_REALLOC( + event, rmw_events.events, rmw_events.event_count) + ); + + return RCL_RET_OK; +} + +rcl_ret_t +rcl_wait_set_add_guard_condition( + rcl_wait_set_t * wait_set, + const rcl_guard_condition_t * guard_condition, + size_t * index) +{ + SET_ADD(guard_condition) + SET_ADD_RMW( + guard_condition, rmw_guard_conditions.guard_conditions, + rmw_guard_conditions.guard_condition_count) + + return RCL_RET_OK; +} + +rcl_ret_t +rcl_wait_set_add_timer( + rcl_wait_set_t * wait_set, + const rcl_timer_t * timer, + size_t * index) +{ + SET_ADD(timer) + // Add timer guard conditions to end of rmw guard condtion set. + rcl_guard_condition_t * guard_condition = rcl_timer_get_guard_condition(timer); + if (NULL != guard_condition) { + // rcl_wait() will take care of moving these backwards and setting guard_condition_count. + const size_t index = wait_set->size_of_guard_conditions + (wait_set->impl->timer_index - 1); + rmw_guard_condition_t * rmw_handle = rcl_guard_condition_get_rmw_handle(guard_condition); + RCL_CHECK_FOR_NULL_WITH_MSG( + rmw_handle, rcl_get_error_string().str, return RCL_RET_ERROR); + wait_set->impl->rmw_guard_conditions.guard_conditions[index] = rmw_handle->data; + } + return RCL_RET_OK; +} + +rcl_ret_t +rcl_wait_set_add_client( + rcl_wait_set_t * wait_set, + const rcl_client_t * client, + size_t * index) +{ + SET_ADD(client) + SET_ADD_RMW(client, rmw_clients.clients, rmw_clients.client_count) + return RCL_RET_OK; +} + +rcl_ret_t +rcl_wait_set_add_service( + rcl_wait_set_t * wait_set, + const rcl_service_t * service, + size_t * index) +{ + SET_ADD(service) + SET_ADD_RMW(service, rmw_services.services, rmw_services.service_count) + return RCL_RET_OK; +} + +rcl_ret_t +rcl_wait_set_add_event( + rcl_wait_set_t * wait_set, + const rcl_event_t * event, + size_t * index) +{ + SET_ADD(event) + SET_ADD_RMW(event, rmw_events.events, rmw_events.event_count) + wait_set->impl->rmw_events.events[current_index] = rmw_handle; + return RCL_RET_OK; +} + +rcl_ret_t +rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) +{ + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT); + if (!rcl_wait_set_is_valid(wait_set)) { + RCL_SET_ERROR_MSG("wait set is invalid"); + return RCL_RET_WAIT_SET_INVALID; + } + if ( + wait_set->size_of_subscriptions == 0 && + wait_set->size_of_guard_conditions == 0 && + wait_set->size_of_timers == 0 && + wait_set->size_of_clients == 0 && + wait_set->size_of_services == 0 && + wait_set->size_of_events == 0) + { + RCL_SET_ERROR_MSG("wait set is empty"); + return RCL_RET_WAIT_SET_EMPTY; + } + // Calculate the timeout argument. + // By default, set the timer to block indefinitely if none of the below conditions are met. + rmw_time_t * timeout_argument = NULL; + rmw_time_t temporary_timeout_storage; + + bool is_timer_timeout = false; + int64_t min_timeout = timeout > 0 ? timeout : INT64_MAX; + { // scope to prevent i from colliding below + uint64_t i = 0; + for (i = 0; i < wait_set->impl->timer_index; ++i) { + if (!wait_set->timers[i]) { + continue; // Skip NULL timers. + } + rmw_guard_conditions_t * rmw_gcs = &(wait_set->impl->rmw_guard_conditions); + size_t gc_idx = wait_set->size_of_guard_conditions + i; + if (NULL != rmw_gcs->guard_conditions[gc_idx]) { + // This timer has a guard condition, so move it to make a legal wait set. + rmw_gcs->guard_conditions[rmw_gcs->guard_condition_count] = + rmw_gcs->guard_conditions[gc_idx]; + ++(rmw_gcs->guard_condition_count); + } + bool is_canceled = false; + rcl_ret_t ret = rcl_timer_is_canceled(wait_set->timers[i], &is_canceled); + if (ret != RCL_RET_OK) { + return ret; // The rcl error state should already be set. + } + if (is_canceled) { + wait_set->timers[i] = NULL; + continue; + } + // use timer time to to set the rmw_wait timeout + // TODO(sloretz) fix spurious wake-ups on ROS_TIME timers with ROS_TIME enabled + int64_t timer_timeout = INT64_MAX; + ret = rcl_timer_get_time_until_next_call(wait_set->timers[i], &timer_timeout); + if (ret != RCL_RET_OK) { + return ret; // The rcl error state should already be set. + } + if (timer_timeout < min_timeout) { + is_timer_timeout = true; + min_timeout = timer_timeout; + } + } + } + + if (timeout == 0) { + // Then it is non-blocking, so set the temporary storage to 0, 0 and pass it. + temporary_timeout_storage.sec = 0; + temporary_timeout_storage.nsec = 0; + timeout_argument = &temporary_timeout_storage; + } else if (timeout > 0 || is_timer_timeout) { + // If min_timeout was negative, we need to wake up immediately. + if (min_timeout < 0) { + min_timeout = 0; + } + temporary_timeout_storage.sec = RCL_NS_TO_S(min_timeout); + temporary_timeout_storage.nsec = min_timeout % 1000000000; + timeout_argument = &temporary_timeout_storage; + } + + // Wait. + rmw_ret_t ret = rmw_wait( + &wait_set->impl->rmw_subscriptions, + &wait_set->impl->rmw_guard_conditions, + &wait_set->impl->rmw_services, + &wait_set->impl->rmw_clients, + &wait_set->impl->rmw_events, + wait_set->impl->rmw_wait_set, + timeout_argument); + + // Items that are not ready will have been set to NULL by rmw_wait. + // We now update our handles accordingly. + + // Check for ready timers + // and set not ready timers (which includes canceled timers) to NULL. + size_t i; + for (i = 0; i < wait_set->impl->timer_index; ++i) { + if (!wait_set->timers[i]) { + continue; + } + bool is_ready = false; + rcl_ret_t ret = rcl_timer_is_ready(wait_set->timers[i], &is_ready); + if (ret != RCL_RET_OK) { + return ret; // The rcl error state should already be set. + } + if (!is_ready) { + wait_set->timers[i] = NULL; + } + } + // Check for timeout, return RCL_RET_TIMEOUT only if it wasn't a timer. + if (ret != RMW_RET_OK && ret != RMW_RET_TIMEOUT) { + RCL_SET_ERROR_MSG(rmw_get_error_string().str); + return RCL_RET_ERROR; + } + // Set corresponding rcl subscription handles NULL. + for (i = 0; i < wait_set->size_of_subscriptions; ++i) { + bool is_ready = wait_set->impl->rmw_subscriptions.subscribers[i] != NULL; + if (!is_ready) { + wait_set->subscriptions[i] = NULL; + } + } + // Set corresponding rcl guard_condition handles NULL. + for (i = 0; i < wait_set->size_of_guard_conditions; ++i) { + bool is_ready = wait_set->impl->rmw_guard_conditions.guard_conditions[i] != NULL; + if (!is_ready) { + wait_set->guard_conditions[i] = NULL; + } + } + // Set corresponding rcl client handles NULL. + for (i = 0; i < wait_set->size_of_clients; ++i) { + bool is_ready = wait_set->impl->rmw_clients.clients[i] != NULL; + if (!is_ready) { + wait_set->clients[i] = NULL; + } + } + // Set corresponding rcl service handles NULL. + for (i = 0; i < wait_set->size_of_services; ++i) { + bool is_ready = wait_set->impl->rmw_services.services[i] != NULL; + if (!is_ready) { + wait_set->services[i] = NULL; + } + } + // Set corresponding rcl event handles NULL. + for (i = 0; i < wait_set->size_of_events; ++i) { + bool is_ready = wait_set->impl->rmw_events.events[i] != NULL; + if (!is_ready) { + wait_set->events[i] = NULL; + } + } + + if (RMW_RET_TIMEOUT == ret && !is_timer_timeout) { + return RCL_RET_TIMEOUT; + } + return RCL_RET_OK; +} + +#ifdef __cplusplus +} +#endif diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt new file mode 100644 index 0000000..a9969f5 --- /dev/null +++ b/test/CMakeLists.txt @@ -0,0 +1,453 @@ +find_package(ament_cmake_gtest REQUIRED) +find_package(launch_testing_ament_cmake REQUIRED) + +find_package(mimick_vendor REQUIRED) +find_package(test_msgs REQUIRED) + +find_package(mimick_vendor REQUIRED) + +find_package(rcpputils REQUIRED) +find_package(rcutils REQUIRED) +find_package(rmw_implementation_cmake REQUIRED) + +find_package(osrf_testing_tools_cpp REQUIRED) + +get_target_property(memory_tools_ld_preload_env_var + osrf_testing_tools_cpp::memory_tools LIBRARY_PRELOAD_ENVIRONMENT_VARIABLE) + +include(cmake/rcl_add_custom_executable.cmake) +include(cmake/rcl_add_custom_gtest.cmake) +include(cmake/rcl_add_custom_launch_test.cmake) + +set(extra_lib_dirs "${rcl_lib_dir}") +add_definitions(-DTEST_RESOURCES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/resources") + +set(DISTRIBUTION "Unknown") +if("${CMAKE_SYSTEM_NAME}" STREQUAL "Linux") + # If we are on Linux, look for /etc/os-release, which has "key=value" items. + # Parse those items, looking for a key named "id" (case-insensitive), and + # if we find it, set DISTRIBUTION to that value. That gives us some idea + # of which Linux distribution we are on. + if(EXISTS "/etc/os-release") + file(STRINGS "/etc/os-release" OS_RELEASE) + foreach(line ${OS_RELEASE}) + string(REGEX REPLACE "^(.*)=.*" "\\1" key "${line}") + string(TOLOWER "${key}" key) + if("${key}" STREQUAL "id") + string(REGEX REPLACE "^.*=(.*)" "\\1" DISTRIBUTION "${line}") + string(TOLOWER "${DISTRIBUTION}" DISTRIBUTION) + break() + endif() + endforeach() + endif() +endif() + +# finding gtest once in the highest scope +# prevents finding it repeatedly in each local scope +ament_find_gtest() + +macro(test_target) + find_package(${rmw_implementation} REQUIRED) + test_target_function() +endmacro() + +function(test_target_function) + message(STATUS "Creating tests for '${rmw_implementation}'") + set(rmw_implementation_env_var RMW_IMPLEMENTATION=${rmw_implementation}) + + # Gtests + + rcl_add_custom_gtest(test_client${target_suffix} + SRCS rcl/test_client.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + + rcl_add_custom_gtest(test_time${target_suffix} + SRCS rcl/test_time.cpp + ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} osrf_testing_tools_cpp::memory_tools + AMENT_DEPENDENCIES ${rmw_implementation} + ) + + rcl_add_custom_gtest(test_timer${target_suffix} + SRCS rcl/test_timer.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} osrf_testing_tools_cpp::memory_tools + AMENT_DEPENDENCIES ${rmw_implementation} + ) + + rcl_add_custom_gtest(test_context${target_suffix} + SRCS rcl/test_context.cpp + ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick osrf_testing_tools_cpp::memory_tools + AMENT_DEPENDENCIES ${rmw_implementation} + ) + + rcl_add_custom_gtest(test_get_node_names${target_suffix} + SRCS rcl/test_get_node_names.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" + ) + + rcl_add_custom_gtest(test_lexer${target_suffix} + SRCS rcl/test_lexer.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} + ) + + rcl_add_custom_gtest(test_lexer_lookahead${target_suffix} + SRCS rcl/test_lexer_lookahead.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" + ) + + rcl_add_custom_gtest(test_graph${target_suffix} + SRCS rcl/test_graph.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + TIMEOUT 120 + ) + + rcl_add_custom_gtest(test_info_by_topic${target_suffix} + SRCS rcl/test_info_by_topic.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} wait_for_entity_helpers + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + + rcl_add_custom_gtest(test_count_matched${target_suffix} + SRCS rcl/test_count_matched.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + + rcl_add_custom_gtest(test_get_actual_qos${target_suffix} + SRCS rcl/test_get_actual_qos.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "test_msgs" "osrf_testing_tools_cpp" + ) + + rcl_add_custom_gtest(test_init${target_suffix} + SRCS rcl/test_init.cpp + ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick osrf_testing_tools_cpp::memory_tools + AMENT_DEPENDENCIES ${rmw_implementation} + ) + + # RHEL does not support mimick's inject_on_return functionality, which + # causes test_node to segfault when it runs. Since RHEL is not a Tier 1 + # platform, just disable the test since we are covering it elsewhere. + if( + "${DISTRIBUTION}" STREQUAL "\"centos\"" OR + "${DISTRIBUTION}" STREQUAL "\"almalinux\"") + set(gtest_filter_env_var "GTEST_FILTER=-TestNodeFixture__*.test_rcl_node_init_with_internal_errors") + else() + set(gtest_filter_env_var "") + endif() + rcl_add_custom_gtest(test_node${target_suffix} + SRCS rcl/test_node.cpp + ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var} ${gtest_filter_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick osrf_testing_tools_cpp::memory_tools + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" + TIMEOUT 240 # Large timeout to wait for fault injection tests + ) + + rcl_add_custom_gtest(test_arguments${target_suffix} + SRCS rcl/test_arguments.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ + LIBRARIES ${PROJECT_NAME} osrf_testing_tools_cpp::memory_tools + AMENT_DEPENDENCIES "osrf_testing_tools_cpp" "rcpputils" + ) + + rcl_add_custom_gtest(test_remap${target_suffix} + SRCS rcl/test_remap.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES "osrf_testing_tools_cpp" + ) + + rcl_add_custom_gtest(test_remap_integration${target_suffix} + SRCS rcl/test_remap_integration.cpp + TIMEOUT 200 + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES "osrf_testing_tools_cpp" "test_msgs" + ) + + rcl_add_custom_gtest(test_guard_condition${target_suffix} + SRCS rcl/test_guard_condition.cpp + ENV ${rmw_implementation_env_var} ${memory_tools_ld_preload_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick osrf_testing_tools_cpp::memory_tools + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" + ) + + rcl_add_custom_gtest(test_publisher${target_suffix} + SRCS rcl/test_publisher.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ + LIBRARIES ${PROJECT_NAME} mimick + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + + rcl_add_custom_gtest(test_publisher_wait_all_ack${target_suffix} + SRCS rcl/test_publisher_wait_all_ack.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ + LIBRARIES ${PROJECT_NAME} mimick wait_for_entity_helpers + AMENT_DEPENDENCIES ${rmw_implementation} + "osrf_testing_tools_cpp" + "rcpputils" + "rcutils" + "rosidl_runtime_c" + "test_msgs" + ) + + rcl_add_custom_gtest(test_service${target_suffix} + SRCS rcl/test_service.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick wait_for_entity_helpers + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + + rcl_add_custom_gtest(test_subscription${target_suffix} + SRCS rcl/test_subscription.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick wait_for_entity_helpers + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + TIMEOUT 120 + ) + # TODO(asorbini) Enable message timestamp tests for rmw_connextdds on Windows + # once clock incompatibilities are resolved. + if(rmw_implementation STREQUAL "rmw_fastrtps_cpp" OR + rmw_implementation STREQUAL "rmw_fastrtps_dynamic_cpp" OR + (rmw_implementation STREQUAL "rmw_connextdds" AND NOT WIN32)) + message(STATUS "Enabling message timestamp test for ${rmw_implementation}") + target_compile_definitions(test_subscription${target_suffix} + PUBLIC "RMW_TIMESTAMPS_SUPPORTED=1" "RMW_RECEIVED_TIMESTAMP_SUPPORTED=1") + target_compile_definitions(test_service${target_suffix} + PUBLIC "RMW_TIMESTAMPS_SUPPORTED=1" "RMW_RECEIVED_TIMESTAMP_SUPPORTED=1") + else() + if(rmw_implementation STREQUAL "rmw_cyclonedds_cpp") + message(STATUS "Enabling only source timestamp test for ${rmw_implementation}") + target_compile_definitions(test_subscription${target_suffix} + PUBLIC "RMW_TIMESTAMPS_SUPPORTED=1") + target_compile_definitions(test_service${target_suffix} + PUBLIC "RMW_TIMESTAMPS_SUPPORTED=1") + else() + message(STATUS "Disabling message timestamp test for ${rmw_implementation}") + endif() + endif() + + rcl_add_custom_gtest(test_events${target_suffix} + SRCS rcl/test_events.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + + rcl_add_custom_gtest(test_wait${target_suffix} + SRCS rcl/test_wait.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" + ) + + rcl_add_custom_gtest(test_logging_rosout${target_suffix} + SRCS rcl/test_logging_rosout.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "rcl_interfaces" + ) + + rcl_add_custom_gtest(test_namespace${target_suffix} + SRCS test_namespace.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + + rcl_add_custom_gtest(test_rmw_impl_id_check_func${target_suffix} + SRCS rcl/test_rmw_impl_id_check_func.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick + AMENT_DEPENDENCIES ${rmw_implementation} + ) + + rcl_add_custom_gtest(test_network_flow_endpoints${target_suffix} + SRCS rcl/test_network_flow_endpoints.cpp + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ + LIBRARIES ${PROJECT_NAME} mimick + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + + # Launch tests + + rcl_add_custom_executable(service_fixture${target_suffix} + SRCS rcl/service_fixture.cpp + LIBRARIES ${PROJECT_NAME} wait_for_entity_helpers + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + + rcl_add_custom_executable(client_fixture${target_suffix} + SRCS rcl/client_fixture.cpp + LIBRARIES ${PROJECT_NAME} wait_for_entity_helpers + AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" + ) + + rcl_add_custom_launch_test(test_services + service_fixture + client_fixture + ENV ${rmw_implementation_env_var} + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + TIMEOUT 15 + ) + + set(SKIP_TEST "") + if(WIN32) + # TODO(dhood): launch does not set the return code correctly for these tests on Windows. + # See https://github.com/ros2/launch/issues/66 + set(SKIP_TEST "SKIP_TEST") + endif() + set(TEST_RMW_IMPL_ID_CHECK_EXECUTABLE_NAME "$") + configure_file( + rcl/test_rmw_impl_id_check.py.in + ${CMAKE_CURRENT_BINARY_DIR}/test_rmw_impl_id_check${target_suffix}.py.configure + @ONLY + ) + file(GENERATE + OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test/test_rmw_impl_id_check${target_suffix}_$.py" + INPUT "${CMAKE_CURRENT_BINARY_DIR}/test_rmw_impl_id_check${target_suffix}.py.configure" + ) + add_launch_test( + "${CMAKE_CURRENT_BINARY_DIR}/test/test_rmw_impl_id_check${target_suffix}_$.py" + TARGET test_rmw_impl_id_check${target_suffix} + APPEND_LIBRARY_DIRS "${extra_lib_dirs}" + ${SKIP_TEST} + ) + if(TEST test_rmw_impl_id_check${target_suffix}) + set_tests_properties( + test_rmw_impl_id_check${target_suffix} + PROPERTIES DEPENDS test_rmw_impl_id_check_exe + ) + endif() + +endfunction() + +# Build simple executable for using in the test_rmw_impl_id_check +add_executable(test_rmw_impl_id_check_exe + rcl/test_rmw_impl_id_check_exe.cpp) +target_link_libraries(test_rmw_impl_id_check_exe ${PROJECT_NAME}) + +# This file is used by many tests, so build it just once +add_library(wait_for_entity_helpers STATIC rcl/wait_for_entity_helpers.cpp) +target_include_directories(wait_for_entity_helpers PRIVATE + ${osrf_testing_tools_cpp_INCLUDE_DIRS}) +target_link_libraries(wait_for_entity_helpers PUBLIC ${PROJECT_NAME}) +target_link_libraries(wait_for_entity_helpers PRIVATE + rcutils::rcutils) + +call_for_each_rmw_implementation(test_target) + +rcl_add_custom_gtest(test_validate_enclave_name + SRCS rcl/test_validate_enclave_name.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick +) + +rcl_add_custom_gtest(test_domain_id + SRCS rcl/test_domain_id.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick +) + +rcl_add_custom_gtest(test_localhost + SRCS rcl/test_localhost.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} +) + +rcl_add_custom_gtest(test_logging + SRCS rcl/test_logging.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick + AMENT_DEPENDENCIES "osrf_testing_tools_cpp" +) + +rcl_add_custom_gtest(test_validate_topic_name + SRCS rcl/test_validate_topic_name.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} +) + +rcl_add_custom_gtest(test_expand_topic_name + SRCS rcl/test_expand_topic_name.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick +) + +rcl_add_custom_gtest(test_security + SRCS rcl/test_security.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick + AMENT_DEPENDENCIES "osrf_testing_tools_cpp" +) + +rcl_add_custom_gtest(test_common + SRCS rcl/test_common.cpp ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/common.c + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + INCLUDE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/../src/rcl/ + LIBRARIES ${PROJECT_NAME} +) + +rcl_add_custom_gtest(test_log_level + SRCS rcl/test_log_level.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} mimick + AMENT_DEPENDENCIES "osrf_testing_tools_cpp" +) + +rcl_add_custom_gtest(test_subscription_content_filter_options + SRCS rcl/test_subscription_content_filter_options.cpp + APPEND_LIBRARY_DIRS ${extra_lib_dirs} + LIBRARIES ${PROJECT_NAME} + AMENT_DEPENDENCIES "osrf_testing_tools_cpp" "test_msgs" +) diff --git a/test/cmake/rcl_add_custom_executable.cmake b/test/cmake/rcl_add_custom_executable.cmake new file mode 100644 index 0000000..66d2374 --- /dev/null +++ b/test/cmake/rcl_add_custom_executable.cmake @@ -0,0 +1,60 @@ +# Copyright 2016 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +if(rcl_add_custom_executable_INCLUDED) + return() +endif() +set(rcl_add_custom_executable_INCLUDED TRUE) + +macro(rcl_add_custom_executable target) + cmake_parse_arguments(_ARG + "TRACE" + "" + "SRCS;INCLUDE_DIRS;LIBRARIES;AMENT_DEPENDENCIES" + ${ARGN}) + if(_ARG_UNPARSED_ARGUMENTS) + message(FATAL_ERROR + "rcl_add_custom_executable() called with unused arguments:" + "${_ARG_UNPARSED_ARGUMENTS}") + endif() + + add_executable(${target} ${_ARG_SRCS}) + + if(_ARG_TRACE) + message(STATUS "rcl_add_custom_executable() Target '${target}':") + endif() + # Add extra include directories, if any. + if(_ARG_INCLUDE_DIRS) + if(_ARG_TRACE) + message(STATUS " rcl_add_custom_executable() INCLUDE_DIRS: ${_ARG_INCLUDE_DIRS}") + endif() + target_include_directories(${target} PUBLIC ${_ARG_INCLUDE_DIRS}) + endif() + # Add extra link libraries, if any. + if(_ARG_LIBRARIES) + if(_ARG_TRACE) + message(STATUS " rcl_add_custom_executable() LIBRARIES: ${_ARG_LIBRARIES}") + endif() + target_link_libraries(${target} ${_ARG_LIBRARIES}) + endif() + # Add extra ament dependencies, if any. + if(_ARG_AMENT_DEPENDENCIES) + if(_ARG_TRACE) + message(STATUS " rcl_add_custom_executable() AMENT_DEPENDENCIES: ${_ARG_AMENT_DEPENDENCIES}") + endif() + ament_target_dependencies(${target} ${_ARG_AMENT_DEPENDENCIES}) + endif() + target_compile_definitions(${target} + PUBLIC "RMW_IMPLEMENTATION=${rmw_implementation}") +endmacro() diff --git a/test/cmake/rcl_add_custom_gtest.cmake b/test/cmake/rcl_add_custom_gtest.cmake new file mode 100644 index 0000000..0ebe4bd --- /dev/null +++ b/test/cmake/rcl_add_custom_gtest.cmake @@ -0,0 +1,108 @@ +# Copyright 2016 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +if(rcl_add_custom_gtest_INCLUDED) + return() +endif() +set(rcl_add_custom_gtest_INCLUDED TRUE) + +# +# Custom macro for adding a gtest in rcl. +# +# It also takes some of the arguments of ament_add_gtest as well as +# INCLUDE_DIRS, LIBRARIES, and AMENT_DEPENDENCIES which are passed to +# target_include_directories(), target_link_libraries(), and +# ament_target_dependencies() respectively. +# +# :param target: the target name which will also be used as the test name +# :type target: string +# :param SRCS: list of source files used to create the gtest +# :type SRCS: list of strings +# :param ENV: list of env vars to set; listed as ``VAR=value`` +# :type ENV: list of strings +# :param APPEND_ENV: list of env vars to append if already set, otherwise set; +# listed as ``VAR=value`` +# :type APPEND_ENV: list of strings +# :param APPEND_LIBRARY_DIRS: list of library dirs to append to the appropriate +# OS specific env var, a la LD_LIBRARY_PATH +# :type APPEND_LIBRARY_DIRS: list of strings +# :param INCLUDE_DIRS: list of include directories to add to the target +# :type INCLUDE_DIRS: list of strings +# :param LIBRARIES: list of libraries to link to the target +# :type LIBRARIES: list of strings +# :param AMENT_DEPENDENCIES: list of depends to pass ament_target_dependencies +# :type AMENT_DEPENDENCIES: list of strings +# +# @public +# +macro(rcl_add_custom_gtest target) + cmake_parse_arguments(_ARG + "SKIP_TEST;TRACE" + "TIMEOUT" + "SRCS;ENV;APPEND_ENV;APPEND_LIBRARY_DIRS;INCLUDE_DIRS;LIBRARIES;AMENT_DEPENDENCIES" + ${ARGN}) + if(_ARG_UNPARSED_ARGUMENTS) + message(FATAL_ERROR "rcl_add_custom_gtest() called with unused arguments: ${_ARG_UNPARSED_ARGUMENTS}") + endif() + if(_ARG_ENV) + set(_ARG_ENV "ENV" ${_ARG_ENV}) + endif() + if(_ARG_APPEND_ENV) + set(_ARG_APPEND_ENV "APPEND_ENV" ${_ARG_APPEND_ENV}) + endif() + if(_ARG_APPEND_LIBRARY_DIRS) + set(_ARG_APPEND_LIBRARY_DIRS "APPEND_LIBRARY_DIRS" ${_ARG_APPEND_LIBRARY_DIRS}) + endif() + if(_ARG_SKIP_TEST) + set(_ARG_SKIP_TEST "SKIP_TEST") + else() + set(_ARG_SKIP_TEST "") + endif() + if(_ARG_TIMEOUT) + set(_ARG_TIMEOUT "TIMEOUT" ${_ARG_TIMEOUT}) + endif() + + # Pass args along to ament_add_gtest(). + ament_add_gtest(${target} ${_ARG_SRCS} ${_ARG_ENV} ${_ARG_APPEND_ENV} ${_ARG_APPEND_LIBRARY_DIRS} + ${_ARG_SKIP_TEST} ${_ARG_TIMEOUT}) + # Check if the target was actually created. + if(TARGET ${target}) + if(_ARG_TRACE) + message(STATUS "rcl_add_custom_gtest() Target '${target}':") + endif() + # Add extra include directories, if any. + if(_ARG_INCLUDE_DIRS) + if(_ARG_TRACE) + message(STATUS " rcl_add_custom_gtest() INCLUDE_DIRS: ${_ARG_INCLUDE_DIRS}") + endif() + target_include_directories(${target} PUBLIC ${_ARG_INCLUDE_DIRS}) + endif() + # Add extra link libraries, if any. + if(_ARG_LIBRARIES) + if(_ARG_TRACE) + message(STATUS " rcl_add_custom_gtest() LIBRARIES: ${_ARG_LIBRARIES}") + endif() + target_link_libraries(${target} ${_ARG_LIBRARIES}) + endif() + # Add extra ament dependencies, if any. + if(_ARG_AMENT_DEPENDENCIES) + if(_ARG_TRACE) + message(STATUS " rcl_add_custom_gtest() AMENT_DEPENDENCIES: ${_ARG_AMENT_DEPENDENCIES}") + endif() + ament_target_dependencies(${target} ${_ARG_AMENT_DEPENDENCIES}) + endif() + target_compile_definitions(${target} + PUBLIC "RMW_IMPLEMENTATION=${rmw_implementation}") + endif() +endmacro() diff --git a/test/cmake/rcl_add_custom_launch_test.cmake b/test/cmake/rcl_add_custom_launch_test.cmake new file mode 100644 index 0000000..d4d2e22 --- /dev/null +++ b/test/cmake/rcl_add_custom_launch_test.cmake @@ -0,0 +1,45 @@ +# Copyright 2016 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +if(rcl_add_custom_launch_test_INCLUDED) + return() +endif() + +find_package(launch_testing_ament_cmake REQUIRED) +set(rcl_add_custom_launch_test_INCLUDED TRUE) + +macro(rcl_add_custom_launch_test test_name executable1 executable2) + set(TEST_NAME "${test_name}") + set(TEST_EXECUTABLE1 "$") + set(TEST_EXECUTABLE1_NAME "${executable1}") + set(TEST_EXECUTABLE2 "$") + set(TEST_EXECUTABLE2_NAME "${executable2}") + configure_file( + rcl/test_two_executables.py.in + ${CMAKE_CURRENT_BINARY_DIR}/${test_name}${target_suffix}.py.configure + @ONLY + ) + file(GENERATE + OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/test/${test_name}${target_suffix}_$.py" + INPUT "${CMAKE_CURRENT_BINARY_DIR}/${test_name}${target_suffix}.py.configure" + ) + add_launch_test( + "${CMAKE_CURRENT_BINARY_DIR}/test/${test_name}${target_suffix}_$.py" + TARGET ${test_name}${target_suffix} + ${ARGN} + ) + if(TEST ${test_name}${target_suffix}) + set_tests_properties(${test_name}${target_suffix} PROPERTIES DEPENDS "${executable1}${target_suffix} ${executable2}${target_suffix}") + endif() +endmacro() diff --git a/test/mocking_utils/patch.hpp b/test/mocking_utils/patch.hpp new file mode 100644 index 0000000..7b0d8d8 --- /dev/null +++ b/test/mocking_utils/patch.hpp @@ -0,0 +1,505 @@ +// 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. + +// Original file taken from: +// https://github.com/ros2/rcutils/blob/master/test/mocking_utils/patch.hpp + +#ifndef MOCKING_UTILS__PATCH_HPP_ +#define MOCKING_UTILS__PATCH_HPP_ + +#define MOCKING_UTILS_SUPPORT_VA_LIST +#if (defined(__aarch64__) || defined(__arm__) || defined(_M_ARM) || defined(__thumb__)) +// In ARM machines, va_list does not define comparison operators +// nor the compiler allows defining them via operator overloads. +// Thus, Mimick argument matching code will not compile. +#undef MOCKING_UTILS_SUPPORT_VA_LIST +#endif + +#ifdef MOCKING_UTILS_SUPPORT_VA_LIST +#include +#endif + +#include +#include +#include +#include + +#include "mimick/mimick.h" + +#include "rcutils/error_handling.h" +#include "rcutils/macros.h" + +namespace mocking_utils +{ + +/// Mimick specific traits for each mocking_utils::Patch instance. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam SignatureT Type of the symbol to be patched. +*/ +template +struct PatchTraits; + +/// Traits specialization for ReturnT(void) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT); +}; + +/// Traits specialization for void(void) free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, void); +}; + +/// Traits specialization for ReturnT(ArgT0) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgT0 Argument type. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0); +}; + +/// Traits specialization for void(ArgT0) free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ArgT0 Argument type. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, void, ArgT0); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1); +}; + +/// Traits specialization for void(ArgT0, ArgT1) free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, void, ArgT0, ArgT1); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2); +}; + +/// Traits specialization for void(ArgT0, ArgT1, ArgT2) free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3) free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3); +}; + +/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3) free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4) +/// free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4); +}; + +/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4) +/// free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define(mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4); +}; + +/// Traits specialization for ReturnT(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5) +/// free functions. +/** + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define( + mock_type, ReturnT, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5); +}; + +/// Traits specialization for void(ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5) +/// free functions. +/** + * Necessary for Mimick macros to adjust accordingly when the return + * type is `void`. + * + * \tparam ID Numerical identifier of the patch. Ought to be unique. + * \tparam ArgTx Argument types. + */ +template +struct PatchTraits +{ + mmk_mock_define( + mock_type, void, ArgT0, ArgT1, ArgT2, ArgT3, ArgT4, ArgT5); +}; + +/// Generic trampoline to wrap generalized callables in plain functions. +/** + * \tparam ID Numerical identifier of this trampoline. Ought to be unique. + * \tparam SignatureT Type of the symbol this trampoline replaces. + */ +template +struct Trampoline; + +/// Trampoline specialization for free functions. +template +struct Trampoline +{ + static ReturnT base(ArgTs... args) + { + return target(std::forward(args)...); + } + + static std::function target; +}; + +template +std::function +Trampoline::target; + +/// Setup trampoline with the given @p target. +/** + * \param[in] target Callable that this trampoline will target. + * \return the plain base function of this trampoline. + * + * \tparam ID Numerical identifier of this trampoline. Ought to be unique. + * \tparam SignatureT Type of the symbol this trampoline replaces. + */ +template +auto prepare_trampoline(std::function target) +{ + Trampoline::target = target; + return Trampoline::base; +} + +/// Patch class for binary API mocking +/** + * Built on top of Mimick, to enable symbol mocking on a per dynamically + * linked binary object basis. + * + * \tparam ID Numerical identifier for this patch. Ought to be unique. + * \tparam SignatureT Type of the symbol to be patched. + */ +template +class Patch; + +/// Patch specialization for ReturnT(ArgTs...) free functions. +/** + * \tparam ID Numerical identifier for this patch. Ought to be unique. + * \tparam ReturnT Return value type. + * \tparam ArgTs Argument types. + */ +template +class Patch +{ +public: + using mock_type = typename PatchTraits::mock_type; + + /// Construct a patch. + /** + * \param[in] target Symbol target string, using Mimick syntax + * i.e. "symbol(@scope)?", where scope may be "self" to target the current + * binary, "lib:library_name" to target a given library, "file:path/to/library" + * to target a given file, or "sym:other_symbol" to target the first library + * that defines said symbol. + * \param[in] proxy An indirection to call the target function. + * This indirection must ensure this call goes through the function's + * trampoline, as setup by the dynamic linker. + * \return a mocking_utils::Patch instance. + */ + explicit Patch(const std::string & target, std::function proxy) + : target_(target), proxy_(proxy) + { + } + + // Copy construction and assignment are disabled. + Patch(const Patch &) = delete; + Patch & operator=(const Patch &) = delete; + + Patch(Patch && other) + { + mock_ = other.mock_; + other.mock_ = nullptr; + } + + Patch & operator=(Patch && other) + { + if (mock_) { + mmk_reset(mock_); + } + mock_ = other.mock_; + other.mock_ = nullptr; + } + + ~Patch() + { + if (mock_) { + mmk_reset(mock_); + } + } + + /// Inject a @p replacement for the patched function. + Patch & then_call(std::function replacement) & + { + replace_with(replacement); + return *this; + } + + /// Inject a @p replacement for the patched function. + Patch && then_call(std::function replacement) && + { + replace_with(replacement); + return std::move(*this); + } + +private: + // Helper for template parameter pack expansion using `mmk_any` + // macro as pattern. + template + T any() {return mmk_any(T);} + + void replace_with(std::function replacement) + { + if (mock_) { + throw std::logic_error("Cannot configure patch more than once"); + } + auto type_erased_trampoline = + reinterpret_cast(prepare_trampoline(replacement)); + auto MMK_MANGLE(mock_type, create) = + PatchTraits::MMK_MANGLE(mock_type, create); + mock_ = mmk_mock(target_.c_str(), mock_type); + mmk_when(proxy_(any()...), .then_call = type_erased_trampoline); + } + + mock_type mock_{nullptr}; + std::string target_; + std::function proxy_; +}; + +/// Make a patch for a `target` function. +/** + * Useful for type deduction during \ref mocking_utils::Patch construction. + * + * \param[in] target Symbol target string, using Mimick syntax. + * \param[in] proxy An indirection to call the target function. + * \return a mocking_utils::Patch instance. + * + * \tparam ID Numerical identifier for this patch. Ought to be unique. + * \tparam SignatureT Type of the function to be patched. + * + * \sa mocking_utils::Patch for further reference. + */ +template +auto make_patch(const std::string & target, std::function proxy) +{ + return Patch(target, proxy); +} + +/// Define a dummy operator `op` for a given `type`. +/** + * Useful to enable patching functions that take arguments whose types + * do not define basic comparison operators, as required by Mimick. +*/ +#define MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(type_, op) \ + template \ + typename std::enable_if::value, bool>::type \ + operator op(const T &, const T &) { \ + return false; \ + } + +/// Get the exact \ref mocking_utils::Patch type for a given `id` and `function`. +/** + * Useful to avoid ignored attribute warnings when using the \b decltype operator. + */ +#define MOCKING_UTILS_PATCH_TYPE(id, function) \ + decltype(mocking_utils::make_patch("", nullptr)) + +/// A transparent forwarding proxy to a given `function`. +/** + * Useful to ensure a call to `function` goes through its trampoline. + */ +#define MOCKING_UTILS_PATCH_PROXY(function) \ + [] (auto && ... args)->decltype(auto) { \ + return function(std::forward(args)...); \ + } + +/// Compute a Mimick symbol target string based on which `function` is to be patched +/// in which `scope`. +#define MOCKING_UTILS_PATCH_TARGET(scope, function) \ + (std::string(RCUTILS_STRINGIFY(function)) + "@" + (scope)) + +/// Prepare a mocking_utils::Patch for patching a `function` in a given `scope` +/// but defer applying any changes. +#define prepare_patch(scope, function) \ + make_patch<__COUNTER__, decltype(function)>( \ + MOCKING_UTILS_PATCH_TARGET(scope, function), MOCKING_UTILS_PATCH_PROXY(function) \ + ) + +/// Patch a `function` with a used-provided `replacement` in a given `scope`. +#define patch(scope, function, replacement) \ + prepare_patch(scope, function).then_call(replacement) + +/// Patch a `function` to always yield a given `return_code` in a given `scope`. +#define patch_and_return(scope, function, return_code) \ + patch(scope, function, [&](auto && ...) {return return_code;}) + +/// Patch a `function` to always yield a given `return_code` in a given `scope`. +#define patch_to_fail(scope, function, error_message, return_code) \ + patch( \ + scope, function, [&](auto && ...) { \ + RCUTILS_SET_ERROR_MSG(error_message); \ + return return_code; \ + }) + +/// Patch a `function` to execute normally but always yield a given `return_code` +/// in a given `scope`. +/** + * \warning On some Linux distributions (e.g. CentOS), pointers to function + * reference their PLT trampolines. In such cases, it is not possible to + * call `function` from within the mock. + */ +#define inject_on_return(scope, function, return_code) \ + patch( \ + scope, function, ([&, base = function](auto && ... __args) { \ + if (base != function) { \ + static_cast(base(std::forward(__args)...)); \ + } else { \ + RCUTILS_SAFE_FWRITE_TO_STDERR( \ + "[WARNING] mocking_utils::inject_on_return() cannot forward call to " \ + "original '" RCUTILS_STRINGIFY(function) "' function before injection\n" \ + " at " __FILE__ ":" RCUTILS_STRINGIFY(__LINE__) "\n"); \ + } \ + return return_code; \ + })) + +} // namespace mocking_utils + +#ifdef MOCKING_UTILS_SUPPORT_VA_LIST +// Define dummy comparison operators for C standard va_list type +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(va_list, >) +#endif + +#endif // MOCKING_UTILS__PATCH_HPP_ diff --git a/test/rcl/allocator_testing_utils.h b/test/rcl/allocator_testing_utils.h new file mode 100644 index 0000000..f3a0d7f --- /dev/null +++ b/test/rcl/allocator_testing_utils.h @@ -0,0 +1,168 @@ +// 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 RCL__ALLOCATOR_TESTING_UTILS_H_ +#define RCL__ALLOCATOR_TESTING_UTILS_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +#include + +#include "rcutils/allocator.h" + +typedef struct __failing_allocator_state +{ + bool is_failing; +} __failing_allocator_state; + +void * +failing_malloc(size_t size, void * state) +{ + if (((__failing_allocator_state *)state)->is_failing) { + return nullptr; + } + return rcutils_get_default_allocator().allocate(size, rcutils_get_default_allocator().state); +} + +void * +failing_realloc(void * pointer, size_t size, void * state) +{ + if (((__failing_allocator_state *)state)->is_failing) { + return nullptr; + } + return rcutils_get_default_allocator().reallocate( + pointer, size, rcutils_get_default_allocator().state); +} + +void +failing_free(void * pointer, void * state) +{ + if (((__failing_allocator_state *)state)->is_failing) { + return; + } + rcutils_get_default_allocator().deallocate(pointer, rcutils_get_default_allocator().state); +} + +void * +failing_calloc(size_t number_of_elements, size_t size_of_element, void * state) +{ + if (((__failing_allocator_state *)state)->is_failing) { + return nullptr; + } + return rcutils_get_default_allocator().zero_allocate( + number_of_elements, size_of_element, rcutils_get_default_allocator().state); +} + +static inline rcutils_allocator_t +get_failing_allocator(void) +{ + static __failing_allocator_state state; + state.is_failing = true; + auto failing_allocator = rcutils_get_default_allocator(); + failing_allocator.allocate = failing_malloc; + failing_allocator.deallocate = failing_free; + failing_allocator.reallocate = failing_realloc; + failing_allocator.zero_allocate = failing_calloc; + failing_allocator.state = &state; + return failing_allocator; +} + +static inline void +set_failing_allocator_is_failing(rcutils_allocator_t & failing_allocator, bool state) +{ + ((__failing_allocator_state *)failing_allocator.state)->is_failing = state; +} + +typedef struct time_bomb_allocator_state +{ + int count_until_failure; +} time_bomb_allocator_state; + +static void * time_bomb_malloc(size_t size, void * state) +{ + time_bomb_allocator_state * time_bomb_state = (time_bomb_allocator_state *)state; + if (time_bomb_state->count_until_failure >= 0 && + time_bomb_state->count_until_failure-- == 0) + { + return nullptr; + } + return rcutils_get_default_allocator().allocate(size, rcutils_get_default_allocator().state); +} + +static void * +time_bomb_realloc(void * pointer, size_t size, void * state) +{ + time_bomb_allocator_state * time_bomb_state = (time_bomb_allocator_state *)state; + if (time_bomb_state->count_until_failure >= 0 && + time_bomb_state->count_until_failure-- == 0) + { + return nullptr; + } + return rcutils_get_default_allocator().reallocate( + pointer, size, rcutils_get_default_allocator().state); +} + +static void +time_bomb_free(void * pointer, void * state) +{ + time_bomb_allocator_state * time_bomb_state = (time_bomb_allocator_state *)state; + if (time_bomb_state->count_until_failure >= 0 && + time_bomb_state->count_until_failure-- == 0) + { + return; + } + rcutils_get_default_allocator().deallocate(pointer, rcutils_get_default_allocator().state); +} + +static void * +time_bomb_calloc(size_t number_of_elements, size_t size_of_element, void * state) +{ + time_bomb_allocator_state * time_bomb_state = (time_bomb_allocator_state *)state; + if (time_bomb_state->count_until_failure >= 0 && + time_bomb_state->count_until_failure-- == 0) + { + return nullptr; + } + return rcutils_get_default_allocator().zero_allocate( + number_of_elements, size_of_element, rcutils_get_default_allocator().state); +} + +static inline rcutils_allocator_t +get_time_bombed_allocator(void) +{ + static time_bomb_allocator_state state; + state.count_until_failure = 1; + auto time_bombed_allocator = rcutils_get_default_allocator(); + time_bombed_allocator.allocate = time_bomb_malloc; + time_bombed_allocator.deallocate = time_bomb_free; + time_bombed_allocator.reallocate = time_bomb_realloc; + time_bombed_allocator.zero_allocate = time_bomb_calloc; + time_bombed_allocator.state = &state; + return time_bombed_allocator; +} + +static inline void +set_time_bombed_allocator_count(rcutils_allocator_t & time_bombed_allocator, int count) +{ + ((time_bomb_allocator_state *)time_bombed_allocator.state)->count_until_failure = count; +} + +#ifdef __cplusplus +} +#endif + +#endif // RCL__ALLOCATOR_TESTING_UTILS_H_ diff --git a/test/rcl/arg_macros.hpp b/test/rcl/arg_macros.hpp new file mode 100644 index 0000000..f730444 --- /dev/null +++ b/test/rcl/arg_macros.hpp @@ -0,0 +1,81 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__ARG_MACROS_HPP_ +#define RCL__ARG_MACROS_HPP_ + +#include "osrf_testing_tools_cpp/scope_exit.hpp" + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcutils/strdup.h" + +/// Helper to get around non-const args passed to rcl_init(). +char ** +copy_args(int argc, const char ** args) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + char ** copy = static_cast(allocator.allocate(sizeof(char *) * argc, allocator.state)); + for (int i = 0; i < argc; ++i) { + copy[i] = rcutils_strdup(args[i], allocator); + } + return copy; +} + +/// Destroy args allocated by copy_args. +void +destroy_args(int argc, char ** args) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + for (int i = 0; i < argc; ++i) { + allocator.deallocate(args[i], allocator.state); + } + allocator.deallocate(args, allocator.state); +} + +#define SCOPE_GLOBAL_ARGS(argc, argv, ...) \ + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); \ + ASSERT_EQ(RCL_RET_OK, rcl_init_options_init(&init_options, rcl_get_default_allocator())) \ + << rcl_get_error_string().str; \ + rcl_context_t context = rcl_get_zero_initialized_context(); \ + { \ + const char * const_argv[] = {__VA_ARGS__}; \ + argc = (sizeof(const_argv) / sizeof(const char *)); \ + argv = copy_args(argc, const_argv); \ + rcl_ret_t ret = rcl_init(argc, argv, &init_options, &context); \ + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + } \ + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( \ + { \ + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; \ + destroy_args(argc, argv); \ + ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context)) << rcl_get_error_string().str; \ + ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context)) << rcl_get_error_string().str; \ + }) + +#define SCOPE_ARGS(local_arguments, ...) \ + { \ + local_arguments = rcl_get_zero_initialized_arguments(); \ + const char * local_argv[] = {__VA_ARGS__}; \ + unsigned int local_argc = (sizeof(local_argv) / sizeof(const char *)); \ + rcl_ret_t ret = rcl_parse_arguments( \ + local_argc, local_argv, rcl_get_default_allocator(), &local_arguments); \ + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + } \ + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( \ + { \ + ASSERT_EQ(RCL_RET_OK, rcl_arguments_fini(&local_arguments)); \ + }) + +#endif // RCL__ARG_MACROS_HPP_ diff --git a/test/rcl/client_fixture.cpp b/test/rcl/client_fixture.cpp new file mode 100644 index 0000000..22142d8 --- /dev/null +++ b/test/rcl/client_fixture.cpp @@ -0,0 +1,152 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rcutils/logging_macros.h" + +#include "rcl/client.h" +#include "rcl/rcl.h" + +#include "test_msgs/srv/basic_types.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "wait_for_entity_helpers.hpp" + +int main(int argc, char ** argv) +{ + int main_ret = 0; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + if (RCL_RET_OK != ret) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in rcl init options init: %s", rcl_get_error_string().str); + return -1; + } + rcl_context_t context = rcl_get_zero_initialized_context(); + if (rcl_init(argc, argv, &init_options, &context) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in rcl init: %s", rcl_get_error_string().str); + return -1; + } + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + if (rcl_shutdown(&context) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error shutting down rcl: %s", rcl_get_error_string().str); + main_ret = -1; + } + if (rcl_context_fini(&context) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error finalizing rcl context: %s", rcl_get_error_string().str); + main_ret = -1; + } + }); + ret = rcl_init_options_fini(&init_options); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in options fini: %s", rcl_get_error_string().str); + return -1; + } + rcl_node_t node = rcl_get_zero_initialized_node(); + const char * name = "client_fixture_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + if (rcl_node_init(&node, name, "", &context, &node_options) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in node init: %s", rcl_get_error_string().str); + return -1; + } + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + if (rcl_node_fini(&node) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in node fini: %s", rcl_get_error_string().str); + main_ret = -1; + } + }); + + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + const char * service_name = "basic_types"; + + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options = rcl_client_get_default_options(); + ret = rcl_client_init(&client, &node, ts, service_name, &client_options); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in client init: %s", rcl_get_error_string().str); + return -1; + } + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + if (rcl_client_fini(&client, &node)) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in client fini: %s", rcl_get_error_string().str); + main_ret = -1; + } + }); + + // Wait until server is available + if (!wait_for_server_to_be_available(&node, &client, 30, 100)) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Server never became available"); + return -1; + } + + // Initialize a request. + test_msgs__srv__BasicTypes_Request client_request; + // TODO(dirk-thomas) zero initialization necessary until + // https://github.com/ros2/ros2/issues/397 is implemented + memset(&client_request, 0, sizeof(test_msgs__srv__BasicTypes_Request)); + test_msgs__srv__BasicTypes_Request__init(&client_request); + client_request.uint8_value = 1; + client_request.uint32_value = 2; + int64_t sequence_number; + + if (rcl_send_request(&client, &client_request, &sequence_number)) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in send request: %s", rcl_get_error_string().str); + return -1; + } + + if (sequence_number != 1) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Got invalid sequence number"); + return -1; + } + + test_msgs__srv__BasicTypes_Request__fini(&client_request); + + // Initialize the response owned by the client and take the response. + test_msgs__srv__BasicTypes_Response client_response; + // TODO(dirk-thomas) zero initialization necessary until + // https://github.com/ros2/ros2/issues/397 is implemented + memset(&client_response, 0, sizeof(test_msgs__srv__BasicTypes_Response)); + test_msgs__srv__BasicTypes_Response__init(&client_response); + + if (!wait_for_client_to_be_ready(&client, &context, 30, 100)) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Client never became ready"); + return -1; + } + rmw_service_info_t header; + if (rcl_take_response_with_info(&client, &header, &client_response) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in send response: %s", rcl_get_error_string().str); + return -1; + } + + test_msgs__srv__BasicTypes_Response__fini(&client_response); + } + + return main_ret; +} diff --git a/test/rcl/failing_allocator_functions.hpp b/test/rcl/failing_allocator_functions.hpp new file mode 100644 index 0000000..8851538 --- /dev/null +++ b/test/rcl/failing_allocator_functions.hpp @@ -0,0 +1,54 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCL__FAILING_ALLOCATOR_FUNCTIONS_HPP_ +#define RCL__FAILING_ALLOCATOR_FUNCTIONS_HPP_ + +#include + +extern "C" +{ +inline +void * +failing_malloc(size_t size, void * state) +{ + (void)size; + (void)state; + return nullptr; +} + +inline +void * +failing_realloc(void * memory_in, size_t size, void * state) +{ + (void)memory_in; + (void)size; + (void)state; + // this is the right fail case, i.e. if failed, the memory_in is not free'd + // see reallocf() for this behavior fix + return nullptr; +} + +inline +void * +failing_calloc(size_t count, size_t size, void * state) +{ + (void)count; + (void)size; + (void)state; + return nullptr; +} +} // extern "C" + +#endif // RCL__FAILING_ALLOCATOR_FUNCTIONS_HPP_ diff --git a/test/rcl/service_fixture.cpp b/test/rcl/service_fixture.cpp new file mode 100644 index 0000000..611cd1f --- /dev/null +++ b/test/rcl/service_fixture.cpp @@ -0,0 +1,156 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "rcutils/logging_macros.h" + +#include "rcl/service.h" + +#include "rcl/rcl.h" + +#include "test_msgs/srv/basic_types.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "wait_for_entity_helpers.hpp" + + +int main(int argc, char ** argv) +{ + int main_ret = 0; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + if (RCL_RET_OK != ret) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in rcl init options init: %s", rcl_get_error_string().str); + return -1; + } + rcl_context_t context = rcl_get_zero_initialized_context(); + if (rcl_init(argc, argv, &init_options, &context) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in rcl init: %s", rcl_get_error_string().str); + return -1; + } + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + if (rcl_shutdown(&context) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error shutting down rcl: %s", rcl_get_error_string().str); + main_ret = -1; + } + if (rcl_context_fini(&context) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error finalizing rcl context: %s", rcl_get_error_string().str); + main_ret = -1; + } + }); + ret = rcl_init_options_fini(&init_options); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in options fini: %s", rcl_get_error_string().str); + return -1; + } + rcl_node_t node = rcl_get_zero_initialized_node(); + const char * name = "service_fixture_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + if (rcl_node_init(&node, name, "", &context, &node_options) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in node init: %s", rcl_get_error_string().str); + return -1; + } + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + if (rcl_node_fini(&node) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in node fini: %s", rcl_get_error_string().str); + main_ret = -1; + } + }); + + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + const char * service_name = "basic_types"; + + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + ret = rcl_service_init(&service, &node, ts, service_name, &service_options); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in service init: %s", rcl_get_error_string().str); + return -1; + } + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + if (rcl_service_fini(&service, &node)) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in service fini: %s", rcl_get_error_string().str); + main_ret = -1; + } + }); + + // Initialize a response. + test_msgs__srv__BasicTypes_Response service_response; + // TODO(dirk-thomas) zero initialization necessary until + // https://github.com/ros2/ros2/issues/397 is implemented + memset(&service_response, 0, sizeof(test_msgs__srv__BasicTypes_Response)); + test_msgs__srv__BasicTypes_Response__init(&service_response); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__srv__BasicTypes_Response__fini(&service_response); + }); + + // Block until a client request comes in. + + if (!wait_for_service_to_be_ready(&service, &context, 30, 100)) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Service never became ready"); + return -1; + } + + // Take the pending request. + test_msgs__srv__BasicTypes_Request service_request; + // TODO(dirk-thomas) zero initialization necessary until + // https://github.com/ros2/ros2/issues/397 is implemented + memset(&service_request, 0, sizeof(test_msgs__srv__BasicTypes_Request)); + test_msgs__srv__BasicTypes_Request__init(&service_request); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__srv__BasicTypes_Request__fini(&service_request); + }); + rmw_request_id_t header; + // TODO(jacquelinekay) May have to check for timeout error codes + if (rcl_take_request(&service, &header, &service_request) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in take_request: %s", rcl_get_error_string().str); + return -1; + } + + // Sum the request and send the response. + service_response.uint64_value = service_request.uint8_value + service_request.uint32_value; + if (rcl_send_response(&service, &header, &service_response) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in send_response: %s", rcl_get_error_string().str); + return -1; + } + // Our scope exits should take care of fini for everything + // stick around until launch gives us a signal to exit + while (true) { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + } + return main_ret; +} diff --git a/test/rcl/test_arguments.cpp b/test/rcl/test_arguments.cpp new file mode 100644 index 0000000..02f8371 --- /dev/null +++ b/test/rcl/test_arguments.cpp @@ -0,0 +1,1328 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcpputils/filesystem_helper.hpp" + +#include "rcl/rcl.h" +#include "rcl/arguments.h" +#include "rcl/error_handling.h" + +#include "rcl_yaml_param_parser/parser.h" + +#include "rcutils/testing/fault_injection.h" + +#include "./allocator_testing_utils.h" +#include "./arguments_impl.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestArgumentsFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + void SetUp() + { + test_path /= "test_arguments"; + } + + void TearDown() + { + } + + rcpputils::fs::path test_path{TEST_RESOURCES_DIRECTORY}; +}; + +#define EXPECT_UNPARSED(parsed_args, ...) \ + do { \ + int expect_unparsed[] = {__VA_ARGS__}; \ + int expect_num_unparsed = sizeof(expect_unparsed) / sizeof(int); \ + rcl_allocator_t alloc = rcl_get_default_allocator(); \ + int actual_num_unparsed = rcl_arguments_get_count_unparsed(&parsed_args); \ + int * actual_unparsed = NULL; \ + if (actual_num_unparsed > 0) { \ + rcl_ret_t ret = rcl_arguments_get_unparsed(&parsed_args, alloc, &actual_unparsed); \ + ASSERT_EQ(RCL_RET_OK, ret); \ + ASSERT_TRUE(NULL != actual_unparsed); \ + } \ + std::stringstream expected; \ + expected << "["; \ + for (int e = 0; e < expect_num_unparsed; ++e) { \ + expected << expect_unparsed[e] << ", "; \ + } \ + expected << "]"; \ + std::stringstream actual; \ + actual << "["; \ + for (int a = 0; a < actual_num_unparsed; ++a) { \ + actual << actual_unparsed[a] << ", "; \ + } \ + actual << "]"; \ + if (NULL != actual_unparsed) { \ + alloc.deallocate(actual_unparsed, alloc.state); \ + } \ + EXPECT_EQ(expected.str(), actual.str()); \ + } while (0) + +#define EXPECT_UNPARSED_ROS(parsed_args, ...) \ + do { \ + int expect_unparsed_ros[] = {__VA_ARGS__}; \ + int expect_num_unparsed_ros = sizeof(expect_unparsed_ros) / sizeof(int); \ + rcl_allocator_t alloc = rcl_get_default_allocator(); \ + int actual_num_unparsed_ros = rcl_arguments_get_count_unparsed_ros(&parsed_args); \ + int * actual_unparsed_ros = NULL; \ + if (actual_num_unparsed_ros > 0) { \ + rcl_ret_t ret = rcl_arguments_get_unparsed_ros(&parsed_args, alloc, &actual_unparsed_ros); \ + ASSERT_EQ(RCL_RET_OK, ret); \ + ASSERT_TRUE(NULL != actual_unparsed_ros); \ + } \ + std::stringstream expected; \ + expected << "["; \ + for (int e = 0; e < expect_num_unparsed_ros; ++e) { \ + expected << expect_unparsed_ros[e] << ", "; \ + } \ + expected << "]"; \ + std::stringstream actual; \ + actual << "["; \ + for (int a = 0; a < actual_num_unparsed_ros; ++a) { \ + actual << actual_unparsed_ros[a] << ", "; \ + } \ + actual << "]"; \ + if (NULL != actual_unparsed_ros) { \ + alloc.deallocate(actual_unparsed_ros, alloc.state); \ + } \ + EXPECT_EQ(expected.str(), actual.str()); \ + } while (0) + +bool +are_known_ros_args(std::vector argv) +{ + const int argc = static_cast(argv.size()); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments( + argc, argv.data(), rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + bool is_valid = ( + 0 == rcl_arguments_get_count_unparsed(&parsed_args) && + 0 == rcl_arguments_get_count_unparsed_ros(&parsed_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + return is_valid; +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), check_known_vs_unknown_args) { + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "__node:=node_name"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "old_name:__node:=node_name"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "old_name:__node:=nodename123"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "__node:=nodename123"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "__ns:=/foo/bar"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "__ns:=/"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "_:=kq"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "nodename:__ns:=/foobar"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "foo:=bar"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "~/foo:=~/bar"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "/foo/bar:=bar"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "foo:=/bar"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "/foo123:=/bar123"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "node:/foo123:=/bar123"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "rostopic:=/foo/bar"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "rosservice:=baz"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "rostopic://rostopic:=rosservice"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "rostopic:///rosservice:=rostopic"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-r", "rostopic:///foo/bar:=baz"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-p", "foo:=bar"})); + // TODO(ivanpauno): Currently, we're accepting `/`, as they're being accepted by qos overrides. + // We might need to revisit qos overrides parameters names if ROS 2 URIs get + // modified. + EXPECT_TRUE( + are_known_ros_args( + {"--ros-args", "-p", "qos_overrides./foo/bar.publisher.history:=keep_last"})); + // TODO(hidmic): restore tests (and drop the following ones) when parameter names + // are standardized to use slashes in lieu of dots. + // EXPECT_TRUE(are_known_ros_args({"--ros-args", "-p", "~/foo:=~/bar"})); + // EXPECT_TRUE(are_known_ros_args({"--ros-args", "-p", "foo:=/bar"})); + // EXPECT_TRUE(are_known_ros_args({"--ros-args", "-p", "/foo123:=/bar123"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-p", "foo.bar:=bar"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-p", "node:foo:=bar"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "-p", "fizz123:=buzz456"})); + + const std::string parameters_filepath = (test_path / "test_parameters.1.yaml").string(); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--params-file", parameters_filepath.c_str()})); + + EXPECT_FALSE(are_known_ros_args({"--ros-args", "--custom-ros-arg"})); + EXPECT_FALSE(are_known_ros_args({"--ros-args", "__node:=node_name"})); + EXPECT_FALSE(are_known_ros_args({"--ros-args", "old_name:__node:=node_name"})); + EXPECT_FALSE(are_known_ros_args({"--ros-args", "/foo/bar:=bar"})); + EXPECT_FALSE(are_known_ros_args({"--ros-args", "foo:=/bar"})); + EXPECT_FALSE(are_known_ros_args({"--ros-args", "file_name.yaml"})); + + // Setting config logging file + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--log-config-file", "file.config"})); + + // Setting logger level + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--log-level", "UNSET"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--log-level", "DEBUG"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--log-level", "INFO"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--log-level", "WARN"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--log-level", "ERROR"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--log-level", "FATAL"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--log-level", "debug"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--log-level", "Info"})); + + EXPECT_FALSE(are_known_ros_args({"--ros-args", "--log", "foo"})); + EXPECT_FALSE(are_known_ros_args({"--ros-args", "--loglevel", "foo"})); + + // Disabling logging + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--enable-rosout-logs"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--disable-rosout-logs"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--enable-stdout-logs"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--disable-stdout-logs"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--enable-external-lib-logs"})); + EXPECT_TRUE(are_known_ros_args({"--ros-args", "--disable-external-lib-logs"})); + + EXPECT_FALSE(are_known_ros_args({"--ros-args", "stdout-logs"})); + EXPECT_FALSE(are_known_ros_args({"--ros-args", "external-lib-logs"})); + EXPECT_FALSE(are_known_ros_args({"--ros-args", "external-lib-logs"})); +} + +bool +are_valid_ros_args(std::vector argv) +{ + const int argc = static_cast(argv.size()); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments( + argc, argv.data(), rcl_get_default_allocator(), &parsed_args); + if (RCL_RET_OK != ret) { + EXPECT_EQ(ret, RCL_RET_INVALID_ROS_ARGS) << rcl_get_error_string().str; + rcl_reset_error(); + return false; + } + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + return true; +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), check_valid_vs_invalid_args) { + const std::string parameters_filepath = (test_path / "test_parameters.1.yaml").string(); + EXPECT_TRUE( + are_valid_ros_args( + { + "--ros-args", "-p", "foo:=bar", "-r", "__node:=node_name", + "--params-file", parameters_filepath.c_str(), "--log-level", "INFO", + "--log-config-file", "file.config" + })); + + // ROS args unknown to rcl are not (necessarily) invalid + EXPECT_TRUE(are_valid_ros_args({"--ros-args", "--custom-ros-arg"})); + + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--remap"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", ":"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "1"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "~"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", ":="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "foo:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", ":=bar"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "::="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "1:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "~:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "__node:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "__node:=/foo/bar"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "__ns:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "__ns:=foo"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", ":__node:=nodename"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "~:__node:=nodename"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "}foo:=/bar"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "f oo:=/bar"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "foo:=/b ar"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "f{oo:=/bar"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "foo:=/b}ar"})); + + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "rostopic://:=rosservice"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-r", "rostopic::=rosservice"})); + + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", ":="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "foo:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", ":=bar"})); + + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", ":"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "1"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "~"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "::="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "1:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "~:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "__node:="})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "__node:=/foo/bar"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "__ns:=foo"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", ":__node:=nodename"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "~:__node:=nodename"})); + + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "}foo:=/bar"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--param", "}foo:=/bar"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-p", "f oo:=/bar"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--param", "f oo:=/bar"})); + + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "-e"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--enclave"})); + + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--params-file"})); + + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--log-config-file"})); + + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--log-level"})); + EXPECT_FALSE(are_valid_ros_args({"--ros-args", "--log-level", "foo"})); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_args) { + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(0, NULL, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(0, rcl_arguments_get_count_unparsed(&parsed_args)); + EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&parsed_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_null_args) { + const int argc = 1; + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, NULL, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_negative_args) { + const int argc = -1; + const char * const argv[] = {"process_name"}; + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_parse_args) { + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_allocator_t bad_alloc = get_failing_allocator(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, bad_alloc, &parsed_args); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_unparse_args) { + const char * const argv[] = { + "process_name", "--ros-args", "/foo/bar:=", "-r", "bar:=/fiz/buz", "}bar:=fiz", "--", "arg"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_allocator_t bad_alloc = get_failing_allocator(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(2, rcl_arguments_get_count_unparsed(&parsed_args)); + + int * actual_unparsed = NULL; + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_arguments_get_unparsed(&parsed_args, bad_alloc, &actual_unparsed)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_arguments_get_unparsed_ros(&parsed_args, bad_alloc, &actual_unparsed)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_arguments_get_unparsed_ros(nullptr, allocator, &actual_unparsed)); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_empty_unparsed) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_arguments_t empty_parsed_args = rcl_get_zero_initialized_arguments(); + int * actual_unparsed = NULL; + int * actual_unparsed_ros = NULL; + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_arguments_get_unparsed(&empty_parsed_args, allocator, &actual_unparsed)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_arguments_get_unparsed_ros(&empty_parsed_args, allocator, &actual_unparsed_ros)); + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_params_get_counts) { + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + EXPECT_EQ(-1, rcl_arguments_get_count_unparsed(nullptr)); + rcl_reset_error(); + EXPECT_EQ(-1, rcl_arguments_get_count_unparsed_ros(nullptr)); + rcl_reset_error(); + EXPECT_EQ(-1, rcl_arguments_get_param_files_count(nullptr)); + rcl_reset_error(); + EXPECT_EQ(-1, rcl_arguments_get_count_unparsed(&parsed_args)); + rcl_reset_error(); + EXPECT_EQ(-1, rcl_arguments_get_count_unparsed_ros(&parsed_args)); + rcl_reset_error(); + EXPECT_EQ(-1, rcl_arguments_get_param_files_count(&parsed_args)); + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_null_args_output) { + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), NULL); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_ros_args) { + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_UNPARSED(parsed_args, 0); + EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&parsed_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_zero_ros_args) { + const char * const argv[] = {"process_name", "--ros-args"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_UNPARSED(parsed_args, 0); + EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&parsed_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_zero_ros_args_w_trailing_dashes) { + const char * const argv[] = {"process_name", "--ros-args", "--"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_UNPARSED(parsed_args, 0); + EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&parsed_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remap) { + const char * const argv[] = { + "process_name", "--ros-args", "-r", "/foo/bar:=/fiz/buz", "--remap", "foo:=/baz" + }; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret; + ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_UNPARSED(parsed_args, 0); + EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&parsed_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_two_ros_args) { + const char * const argv[] = + {"process_name", "--ros-args", "--ros-args", "-r", "/foo/bar:=/fiz/buz"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret; + ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_UNPARSED(parsed_args, 0); + EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&parsed_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_w_trailing_dashes) { + const char * const argv[] = {"process_name", "--ros-args", "-r", "/foo/bar:=/fiz/buz", "--"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret; + ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_UNPARSED(parsed_args, 0); + EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&parsed_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_one_remap_w_two_trailing_dashes) { + const char * const argv[] = + {"process_name", "--ros-args", "-r", "/foo/bar:=/fiz/buz", "--", "--"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret; + ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_UNPARSED(parsed_args, 0, 5); + EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&parsed_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_mix_valid_invalid_rules) { + const char * const argv[] = { + "process_name", "--ros-args", "/foo/bar:=", "-r", "bar:=/fiz/buz", "}bar:=fiz", "--", "arg" + }; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret; + ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_UNPARSED(parsed_args, 0, 7); + EXPECT_UNPARSED_ROS(parsed_args, 2, 5); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy) { + const char * const argv[] = { + "process_name", "--ros-args", "/foo/bar:=", "-r", "bar:=/fiz/buz", "-r", "__ns:=/foo", "--", + "arg" + }; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret; + + ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_arguments_t copied_args = rcl_get_zero_initialized_arguments(); + ret = rcl_arguments_copy(&parsed_args, &copied_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Can't copy to non empty + ret = rcl_arguments_copy(&parsed_args, &copied_args); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + EXPECT_UNPARSED(parsed_args, 0, 8); + EXPECT_UNPARSED_ROS(parsed_args, 2); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + + EXPECT_UNPARSED(copied_args, 0, 8); + EXPECT_UNPARSED_ROS(copied_args, 2); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&copied_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy_bad_alloc) { + const char * const argv[] = {"process_name", "--ros-args", "/foo/bar:="}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret; + + ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_arguments_t copied_args = rcl_get_zero_initialized_arguments(); + rcl_allocator_t bad_alloc = get_failing_allocator(); + rcl_allocator_t saved_alloc = parsed_args.impl->allocator; + parsed_args.impl->allocator = bad_alloc; + ret = rcl_arguments_copy(&parsed_args, &copied_args); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + rcl_reset_error(); + parsed_args.impl->allocator = saved_alloc; + + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)) << rcl_get_error_string().str; +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy_no_ros_args) { + const char * const argv[] = {"process_name", "--ros-args", "--", "arg", "--ros-args"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret; + + ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_arguments_t copied_args = rcl_get_zero_initialized_arguments(); + ret = rcl_arguments_copy(&parsed_args, &copied_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_UNPARSED(parsed_args, 0, 3); + EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&parsed_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + + EXPECT_UNPARSED(copied_args, 0, 3); + EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&copied_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&copied_args)); +} + +// Similar to the default allocator, but returns NULL when size is zero. +// This is useful for emulating systems where `malloc(0)` return NULL. +// TODO(jacobperron): Consider using this allocate function in other tests +static void * +__return_null_on_zero_allocate(size_t size, void * state) +{ + RCUTILS_UNUSED(state); + if (size == 0) { + return NULL; + } + return malloc(size); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy_no_args) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + allocator.allocate = __return_null_on_zero_allocate; + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(0, NULL, allocator, &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(0, rcl_arguments_get_count_unparsed(&parsed_args)); + EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&parsed_args)); + + rcl_arguments_t copied_args = rcl_get_zero_initialized_arguments(); + ret = rcl_arguments_copy(&parsed_args, &copied_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(0, rcl_arguments_get_count_unparsed(&copied_args)); + EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&copied_args)); + + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&copied_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_two_namespace) { + const char * const argv[] = { + "process_name", "--ros-args", "-r", "__ns:=/foo/bar", "-r", "__ns:=/fiz/buz" + }; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret; + ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_UNPARSED(parsed_args, 0); + EXPECT_EQ(0, rcl_arguments_get_count_unparsed_ros(&parsed_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_uninitialized_parsed_args) { + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args; + int not_null = 1; + parsed_args.impl = reinterpret_cast(¬_null); + ASSERT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args)); + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_double_parse) { + const char * const argv[] = { + "process_name", "--ros-args", "-r", "__ns:=/foo/bar", "-r", "__ns:=/fiz/buz" + }; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + ASSERT_EQ( + RCL_RET_OK, + rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + ASSERT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args)); + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_fini_null) { + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_arguments_fini(NULL)); + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_fini_impl_null) { + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + parsed_args.impl = NULL; + EXPECT_EQ(RCL_RET_ERROR, rcl_arguments_fini(&parsed_args)); + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_fini_twice) { + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + ASSERT_EQ(RCL_RET_OK, rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + EXPECT_EQ(RCL_RET_ERROR, rcl_arguments_fini(&parsed_args)); + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_remove_ros_args) { + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_allocator_t default_allocator = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, default_allocator, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + const char ** nonros_argv = NULL; + int nonros_argc = 0; + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_remove_ros_arguments( + NULL, &parsed_args, default_allocator, &nonros_argc, &nonros_argv)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_remove_ros_arguments( + argv, NULL, default_allocator, &nonros_argc, &nonros_argv)); + rcl_reset_error(); + + rcl_allocator_t zero_initialized_allocator = + (rcl_allocator_t)rcutils_get_zero_initialized_allocator(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_remove_ros_arguments( + argv, &parsed_args, zero_initialized_allocator, &nonros_argc, &nonros_argv)); + rcl_reset_error(); + + rcl_allocator_t bad_alloc = get_failing_allocator(); + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_remove_ros_arguments( + argv, &parsed_args, bad_alloc, &nonros_argc, &nonros_argv)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_remove_ros_arguments( + argv, &parsed_args, default_allocator, NULL, &nonros_argv)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_remove_ros_arguments( + argv, &parsed_args, default_allocator, &nonros_argc, NULL)); + rcl_reset_error(); + + rcl_arguments_t zero_initialized_parsed_args = rcl_get_zero_initialized_arguments(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_remove_ros_arguments( + argv, &zero_initialized_parsed_args, default_allocator, &nonros_argc, &nonros_argv)); + rcl_reset_error(); + + const char * stack_allocated_nonros_argv[] = {"--foo", "--bar"}; + const char ** initialized_nonros_argv = stack_allocated_nonros_argv; + int initialized_nonros_argc = sizeof(stack_allocated_nonros_argv) / sizeof(const char *); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_remove_ros_arguments( + argv, &parsed_args, default_allocator, &initialized_nonros_argc, &initialized_nonros_argv)); + rcl_reset_error(); + + rcl_arguments_t no_parsed_args = rcl_get_zero_initialized_arguments(); + ret = rcl_parse_arguments(0, NULL, default_allocator, &no_parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&no_parsed_args)); + }); + + ret = rcl_remove_ros_arguments( + NULL, &no_parsed_args, default_allocator, &nonros_argc, &nonros_argv); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(NULL == nonros_argv); + EXPECT_EQ(0, nonros_argc); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_remove_ros_args) { + const char * const argv[] = { + "process_name", "-d", "--ros-args", "-r", "__ns:=/foo/bar", "-r", "__ns:=/fiz/buz", "--", + "--foo=bar", "--baz", "--ros-args", "--ros-args", "-p", "bar:=baz", "--", "--", "arg", + }; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + int nonros_argc = 0; + const char ** nonros_argv = NULL; + rcl_allocator_t bomb_alloc = get_time_bombed_allocator(); + set_time_bombed_allocator_count(bomb_alloc, 1); + ret = rcl_remove_ros_arguments( + argv, + &parsed_args, + bomb_alloc, + &nonros_argc, + &nonros_argv); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remove_ros_args) { + const char * const argv[] = { + "process_name", "-d", "--ros-args", "-r", "__ns:=/foo/bar", "-r", "__ns:=/fiz/buz", "--", + "--foo=bar", "--baz", "--ros-args", "--ros-args", "-p", "bar:=baz", "--", "--", "arg", + }; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + int nonros_argc = 0; + const char ** nonros_argv = NULL; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + alloc.deallocate(nonros_argv, alloc.state); + }); + + ret = rcl_remove_ros_arguments( + argv, + &parsed_args, + alloc, + &nonros_argc, + &nonros_argv); + + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(nonros_argc, 6); + EXPECT_STREQ(nonros_argv[0], "process_name"); + EXPECT_STREQ(nonros_argv[1], "-d"); + EXPECT_STREQ(nonros_argv[2], "--foo=bar"); + EXPECT_STREQ(nonros_argv[3], "--baz"); + EXPECT_STREQ(nonros_argv[4], "--"); + EXPECT_STREQ(nonros_argv[5], "arg"); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remove_ros_args_if_ros_only) { + const char * const argv[] = {"--ros-args", "--disable-rosout-logs"}; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + int nonros_argc = 0; + const char ** nonros_argv = NULL; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + alloc.deallocate(nonros_argv, alloc.state); + }); + + ret = rcl_remove_ros_arguments( + argv, + &parsed_args, + alloc, + &nonros_argc, + &nonros_argv); + + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_EQ(0, nonros_argc); + EXPECT_TRUE(NULL == nonros_argv); +} + + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_remove_ros_args_if_no_args) { + const char ** argv = NULL; + const int argc = 0; + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + int nonros_argc = 0; + const char ** nonros_argv = NULL; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + alloc.deallocate(nonros_argv, alloc.state); + }); + + ret = rcl_remove_ros_arguments( + argv, + &parsed_args, + alloc, + &nonros_argc, + &nonros_argv); + + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_EQ(0, nonros_argc); + EXPECT_TRUE(NULL == nonros_argv); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_zero) { + const char * const argv[] = + {"process_name", "--ros-args", "-r", "__ns:=/namespace", "random:=arg"}; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + int parameter_filecount = rcl_arguments_get_param_files_count(&parsed_args); + EXPECT_EQ(0, parameter_filecount); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_single) { + const std::string parameters_filepath = (test_path / "test_parameters.1.yaml").string(); + const char * const argv[] = { + "process_name", "--ros-args", "-r", "__ns:=/namespace", "random:=arg", + "--params-file", parameters_filepath.c_str() + }; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + int parameter_filecount = rcl_arguments_get_param_files_count(&parsed_args); + EXPECT_EQ(1, parameter_filecount); + char ** parameter_files = NULL; + ret = rcl_arguments_get_param_files(&parsed_args, alloc, ¶meter_files); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ(parameters_filepath.c_str(), parameter_files[0]); + + for (int i = 0; i < parameter_filecount; ++i) { + alloc.deallocate(parameter_files[i], alloc.state); + } + alloc.deallocate(parameter_files, alloc.state); + + // Test bad alloc + rcl_allocator_t bad_alloc = get_failing_allocator(); + rcl_params_t * params_test = NULL; + rcl_allocator_t saved_alloc = parsed_args.impl->allocator; + parsed_args.impl->parameter_overrides->allocator = bad_alloc; + ret = rcl_arguments_get_param_overrides(&parsed_args, ¶ms_test); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + EXPECT_EQ(NULL, params_test); + parsed_args.impl->parameter_overrides->allocator = saved_alloc; + + // Expected usage + rcl_params_t * params = NULL; + ret = rcl_arguments_get_param_overrides(&parsed_args, ¶ms); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_yaml_node_struct_fini(params); + }); + EXPECT_EQ(1U, params->num_nodes); + + rcl_variant_t * param_value = + rcl_yaml_node_struct_get("some_node", "param_group.string_param", params); + ASSERT_TRUE(NULL != param_value); + ASSERT_TRUE(NULL != param_value->string_value); + EXPECT_STREQ("foo", param_value->string_value); + + param_value = rcl_yaml_node_struct_get("some_node", "int_param", params); + ASSERT_TRUE(NULL != param_value); + ASSERT_TRUE(NULL != param_value->integer_value); + EXPECT_EQ(1, *(param_value->integer_value)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_argument_multiple) { + const std::string parameters_filepath1 = (test_path / "test_parameters.1.yaml").string(); + const std::string parameters_filepath2 = (test_path / "test_parameters.2.yaml").string(); + const char * const argv[] = { + "process_name", "--ros-args", "--params-file", parameters_filepath1.c_str(), + "-r", "__ns:=/namespace", "random:=arg", "--params-file", parameters_filepath2.c_str() + }; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_ret_t ret; + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + + ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + int parameter_filecount = rcl_arguments_get_param_files_count(&parsed_args); + EXPECT_EQ(2, parameter_filecount); + char ** parameter_files = NULL; + ret = rcl_arguments_get_param_files(&parsed_args, alloc, ¶meter_files); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ(parameters_filepath1.c_str(), parameter_files[0]); + EXPECT_STREQ(parameters_filepath2.c_str(), parameter_files[1]); + for (int i = 0; i < parameter_filecount; ++i) { + alloc.deallocate(parameter_files[i], alloc.state); + } + + alloc.deallocate(parameter_files, alloc.state); + + rcl_params_t * params = NULL; + ret = rcl_arguments_get_param_overrides(&parsed_args, ¶ms); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_yaml_node_struct_fini(params); + }); + EXPECT_EQ(2U, params->num_nodes); + + rcl_variant_t * param_value = + rcl_yaml_node_struct_get("some_node", "int_param", params); + ASSERT_TRUE(NULL != param_value); + ASSERT_TRUE(NULL != param_value->integer_value); + EXPECT_EQ(3, *(param_value->integer_value)); + + param_value = rcl_yaml_node_struct_get("some_node", "param_group.string_param", params); + ASSERT_TRUE(NULL != param_value); + ASSERT_TRUE(NULL != param_value->string_value); + EXPECT_STREQ("foo", param_value->string_value); + + param_value = rcl_yaml_node_struct_get("another_node", "double_param", params); + ASSERT_TRUE(NULL != param_value); + ASSERT_TRUE(NULL != param_value->double_value); + EXPECT_DOUBLE_EQ(1.0, *(param_value->double_value)); + + param_value = rcl_yaml_node_struct_get("another_node", "param_group.bool_array_param", params); + ASSERT_TRUE(NULL != param_value); + ASSERT_TRUE(NULL != param_value->bool_array_value); + ASSERT_TRUE(NULL != param_value->bool_array_value->values); + ASSERT_EQ(3U, param_value->bool_array_value->size); + bool bool_value = param_value->bool_array_value->values[0]; + EXPECT_TRUE(bool_value); + bool_value = param_value->bool_array_value->values[1]; + EXPECT_FALSE(bool_value); + bool_value = param_value->bool_array_value->values[2]; + EXPECT_FALSE(bool_value); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_arguments_copy) { + const std::string parameters_filepath1 = (test_path / "test_parameters.1.yaml").string(); + const std::string parameters_filepath2 = (test_path / "test_parameters.2.yaml").string(); + const char * const argv[] = { + "process_name", "--ros-args", "--params-file", parameters_filepath1.c_str(), + "-r", "__ns:=/namespace", "random:=arg", "--params-file", parameters_filepath2.c_str() + }; + const int argc = sizeof(argv) / sizeof(const char *); + rcl_ret_t ret; + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + + ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + int parameter_filecount = rcl_arguments_get_param_files_count(&parsed_args); + EXPECT_EQ(2, parameter_filecount); + + rcl_arguments_t copied_args = rcl_get_zero_initialized_arguments(); + ret = rcl_arguments_copy(&parsed_args, &copied_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(2, rcl_arguments_get_param_files_count(&copied_args)); + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&copied_args)); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_no_param_overrides) { + const char * const argv[] = {"process_name"}; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + ret = rcl_arguments_get_param_overrides(&parsed_args, NULL); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + rcl_params_t * params = NULL; + ret = rcl_arguments_get_param_overrides(NULL, ¶ms); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + rcl_arguments_t empty_parsed_arg = rcl_get_zero_initialized_arguments(); + ret = rcl_arguments_get_param_overrides(&empty_parsed_arg, ¶ms); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + rcl_params_t preallocated_params; + params = &preallocated_params; + ret = rcl_arguments_get_param_overrides(&parsed_args, ¶ms); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + params = NULL; + ret = rcl_arguments_get_param_overrides(&parsed_args, ¶ms); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(NULL == params); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_param_overrides) { + const std::string parameters_filepath = (test_path / "test_parameters.1.yaml").string(); + const char * const argv[] = { + "process_name", "--ros-args", + "--params-file", parameters_filepath.c_str(), + "--param", "string_param:=bar", + "-p", "some.bool_param:=false", + "-p", "some_node:int_param:=4" + }; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + rcl_params_t * params = NULL; + ret = rcl_arguments_get_param_overrides(&parsed_args, ¶ms); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_yaml_node_struct_fini(params); + }); + EXPECT_EQ(2U, params->num_nodes); + + rcl_variant_t * param_value = + rcl_yaml_node_struct_get("/**", "string_param", params); + ASSERT_TRUE(NULL != param_value); + ASSERT_TRUE(NULL != param_value->string_value); + EXPECT_STREQ("bar", param_value->string_value); + + param_value = rcl_yaml_node_struct_get("/**", "some.bool_param", params); + ASSERT_TRUE(NULL != param_value); + ASSERT_TRUE(NULL != param_value->bool_value); + bool bool_value = *param_value->bool_value; + EXPECT_FALSE(bool_value); + + param_value = rcl_yaml_node_struct_get("some_node", "int_param", params); + ASSERT_TRUE(NULL != param_value); + ASSERT_TRUE(NULL != param_value->integer_value); + EXPECT_EQ(4, *(param_value->integer_value)); + + param_value = rcl_yaml_node_struct_get("some_node", "param_group.string_param", params); + ASSERT_TRUE(NULL != param_value); + ASSERT_TRUE(NULL != param_value->string_value); + EXPECT_STREQ("foo", param_value->string_value); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_bad_alloc_get_param_files) { + const std::string parameters_filepath1 = (test_path / "test_parameters.1.yaml").string(); + const std::string parameters_filepath2 = (test_path / "test_parameters.2.yaml").string(); + const char * const argv[] = { + "process_name", "--ros-args", "--params-file", parameters_filepath1.c_str(), + "-r", "__ns:=/namespace", "random:=arg", "--params-file", parameters_filepath2.c_str() + }; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + int parameter_filecount = rcl_arguments_get_param_files_count(&parsed_args); + EXPECT_EQ(2, parameter_filecount); + + // Configure allocator to fail at different points of the code + rcl_allocator_t bomb_alloc = get_time_bombed_allocator(); + set_time_bombed_allocator_count(bomb_alloc, 0); + char ** parameter_files = NULL; + ret = rcl_arguments_get_param_files(&parsed_args, bomb_alloc, ¶meter_files); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + + set_time_bombed_allocator_count(bomb_alloc, 1); + ret = rcl_arguments_get_param_files(&parsed_args, bomb_alloc, ¶meter_files); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + + set_time_bombed_allocator_count(bomb_alloc, 2); + ret = rcl_arguments_get_param_files(&parsed_args, bomb_alloc, ¶meter_files); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_null_get_param_files) { + const std::string parameters_filepath1 = (test_path / "test_parameters.1.yaml").string(); + const char * const argv[] = { + "process_name", "--ros-args", "--params-file", parameters_filepath1.c_str() + }; + const int argc = sizeof(argv) / sizeof(const char *); + + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + char ** parameter_files = NULL; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_parse_arguments(argc, argv, allocator, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + ret = rcl_arguments_get_param_files(nullptr, allocator, ¶meter_files); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + ret = rcl_arguments_get_param_files(&parsed_args, allocator, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + rcl_arguments_t empty_parsed_args = rcl_get_zero_initialized_arguments(); + ret = rcl_arguments_get_param_files(&empty_parsed_args, allocator, ¶meter_files); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_parse_with_internal_errors) { + const std::string parameters_filepath1 = + (test_path / "test_parameters.1.yaml").string(); + const std::string parameters_filepath2 = + (test_path / "test_parameters.2.yaml").string(); + const char * const argv[] = { + "process_name", RCL_ROS_ARGS_FLAG, + RCL_PARAM_FILE_FLAG, parameters_filepath1.c_str(), + RCL_REMAP_FLAG, "that_node:foo:=baz", + RCL_REMAP_FLAG, "foo:=bar", + RCL_PARAM_FILE_FLAG, parameters_filepath2.c_str(), + RCL_REMAP_FLAG, "__name:=my_node", + RCL_REMAP_FLAG, "__ns:=/my_ns", + RCL_PARAM_FLAG, "testing:=true", + RCL_PARAM_FLAG, "this_node:constant:=42", + RCL_ENCLAVE_FLAG, "fizz", + RCL_ENCLAVE_FLAG, "buzz", // override + RCL_LOG_LEVEL_FLAG, "rcl:=debug", + RCL_EXTERNAL_LOG_CONFIG_FLAG, "flip.txt", + RCL_EXTERNAL_LOG_CONFIG_FLAG, "flop.txt", // override + "--enable-" RCL_LOG_STDOUT_FLAG_SUFFIX, + "--enable-" RCL_LOG_ROSOUT_FLAG_SUFFIX, + "--disable-" RCL_LOG_EXT_LIB_FLAG_SUFFIX, + "--not-a-real-ros-flag", "not-a-real-ros-arg", + RCL_ROS_ARGS_EXPLICIT_END_TOKEN, + "--some-non-ros-flag", "some-non-ros-flag" + }; + const int argc = sizeof(argv) / sizeof(argv[0]); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + RCUTILS_FAULT_INJECTION_TEST( + { + rcl_ret_t ret = rcl_parse_arguments(argc, argv, allocator, &parsed_args); + if (RCL_RET_OK == ret) { + int64_t count = rcutils_fault_injection_get_count(); + rcutils_fault_injection_set_count(RCUTILS_FAULT_INJECTION_NEVER_FAIL); + ret = rcl_arguments_fini(&parsed_args); + rcutils_fault_injection_set_count(count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } else { + rcl_reset_error(); + } + }); +} + +TEST_F(CLASSNAME(TestArgumentsFixture, RMW_IMPLEMENTATION), test_copy_with_internal_errors) { + const std::string parameters_filepath1 = + (test_path / "test_parameters.1.yaml").string(); + const std::string parameters_filepath2 = + (test_path / "test_parameters.2.yaml").string(); + const char * const argv[] = { + "process_name", RCL_ROS_ARGS_FLAG, + RCL_PARAM_FILE_FLAG, parameters_filepath1.c_str(), + RCL_REMAP_FLAG, "that_node:foo:=baz", + RCL_REMAP_FLAG, "foo:=bar", + RCL_PARAM_FILE_FLAG, parameters_filepath2.c_str(), + RCL_REMAP_FLAG, "__name:=my_node", + RCL_REMAP_FLAG, "__ns:=/my_ns", + RCL_PARAM_FLAG, "testing:=true", + RCL_PARAM_FLAG, "this_node:constant:=42", + RCL_ENCLAVE_FLAG, "fizz", + RCL_ENCLAVE_FLAG, "buzz", // override + RCL_LOG_LEVEL_FLAG, "rcl:=debug", + RCL_EXTERNAL_LOG_CONFIG_FLAG, "flip.txt", + RCL_EXTERNAL_LOG_CONFIG_FLAG, "flop.txt", // override + "--enable-" RCL_LOG_STDOUT_FLAG_SUFFIX, + "--enable-" RCL_LOG_ROSOUT_FLAG_SUFFIX, + "--disable-" RCL_LOG_EXT_LIB_FLAG_SUFFIX, + "--not-a-real-ros-flag", "not-a-real-ros-arg", + RCL_ROS_ARGS_EXPLICIT_END_TOKEN, + "--some-non-ros-flag", "some-non-ros-flag" + }; + const int argc = sizeof(argv) / sizeof(argv[0]); + + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + rcl_arguments_t copied_args = rcl_get_zero_initialized_arguments(); + RCUTILS_FAULT_INJECTION_TEST( + { + rcl_ret_t ret = rcl_arguments_copy(&parsed_args, &copied_args); + if (RCL_RET_OK == ret) { + int64_t count = rcutils_fault_injection_get_count(); + rcutils_fault_injection_set_count(RCUTILS_FAULT_INJECTION_NEVER_FAIL); + ret = rcl_arguments_fini(&copied_args); + rcutils_fault_injection_set_count(count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } else { + rcl_reset_error(); + } + }); +} diff --git a/test/rcl/test_client.cpp b/test/rcl/test_client.cpp new file mode 100644 index 0000000..9c3330b --- /dev/null +++ b/test/rcl/test_client.cpp @@ -0,0 +1,330 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/client.h" + +#include "rcl/rcl.h" +#include "rcutils/testing/fault_injection.h" + +#include "test_msgs/srv/basic_types.h" + +#include "./failing_allocator_functions.hpp" +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" + +class TestClientFixture : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "test_client_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(ret, RCL_RET_OK); + ret = rcl_context_fini(this->context_ptr); + EXPECT_EQ(ret, RCL_RET_OK); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +/* Basic nominal test of a client. Complete functionality tested at test_service.cpp + */ +TEST_F(TestClientFixture, test_client_nominal) { + rcl_ret_t ret; + rcl_client_t client = rcl_get_zero_initialized_client(); + + // Initialize the client. + const char * topic_name = "add_two_ints"; + const char * expected_topic_name = "/add_two_ints"; + rcl_client_options_t client_options = rcl_client_get_default_options(); + + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + ret = rcl_client_init(&client, this->node_ptr, ts, topic_name, &client_options); + + // Test access to client options + const rcl_client_options_t * client_internal_options = rcl_client_get_options(&client); + EXPECT_TRUE(rcutils_allocator_is_valid(&(client_internal_options->allocator))); + EXPECT_EQ(rmw_qos_profile_services_default.reliability, client_internal_options->qos.reliability); + EXPECT_EQ(rmw_qos_profile_services_default.history, client_internal_options->qos.history); + EXPECT_EQ(rmw_qos_profile_services_default.depth, client_internal_options->qos.depth); + EXPECT_EQ(rmw_qos_profile_services_default.durability, client_internal_options->qos.durability); + + const rmw_qos_profile_t * request_publisher_qos = + rcl_client_request_publisher_get_actual_qos(&client); + EXPECT_EQ(rmw_qos_profile_services_default.reliability, request_publisher_qos->reliability); + EXPECT_EQ(rmw_qos_profile_services_default.history, request_publisher_qos->history); + EXPECT_EQ(rmw_qos_profile_services_default.depth, request_publisher_qos->depth); + EXPECT_EQ(rmw_qos_profile_services_default.durability, request_publisher_qos->durability); + EXPECT_EQ( + rmw_qos_profile_services_default.avoid_ros_namespace_conventions, + request_publisher_qos->avoid_ros_namespace_conventions); + + const rmw_qos_profile_t * response_subscription_qos = + rcl_client_response_subscription_get_actual_qos(&client); + EXPECT_EQ(rmw_qos_profile_services_default.reliability, response_subscription_qos->reliability); + EXPECT_EQ(rmw_qos_profile_services_default.history, response_subscription_qos->history); + EXPECT_EQ(rmw_qos_profile_services_default.depth, response_subscription_qos->depth); + EXPECT_EQ(rmw_qos_profile_services_default.durability, response_subscription_qos->durability); + EXPECT_EQ( + rmw_qos_profile_services_default.avoid_ros_namespace_conventions, + response_subscription_qos->avoid_ros_namespace_conventions); + + // Check the return code of initialization and that the service name matches what's expected + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(strcmp(rcl_client_get_service_name(&client), expected_topic_name), 0); + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + // Initialize the client request. + test_msgs__srv__BasicTypes_Request req; + test_msgs__srv__BasicTypes_Request__init(&req); + req.uint8_value = 1; + req.uint32_value = 2; + + // Check that there were no errors while sending the request. + int64_t sequence_number = 0; + ret = rcl_send_request(&client, &req, &sequence_number); + EXPECT_EQ(sequence_number, 1); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + test_msgs__srv__BasicTypes_Request__fini(&req); +} + + +/* Testing the client init and fini functions. + */ +TEST_F(TestClientFixture, test_client_init_fini) { + rcl_ret_t ret; + // Setup valid inputs. + rcl_client_t client; + + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + const char * topic_name = "chatter"; + rcl_client_options_t default_client_options = rcl_client_get_default_options(); + + // Try passing null for client in init. + ret = rcl_client_init(nullptr, this->node_ptr, ts, topic_name, &default_client_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing null for a node pointer in init. + client = rcl_get_zero_initialized_client(); + ret = rcl_client_init(&client, nullptr, ts, topic_name, &default_client_options); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Check if null publisher is valid + EXPECT_FALSE(rcl_client_is_valid(nullptr)); + rcl_reset_error(); + + // Check if zero initialized client is valid + client = rcl_get_zero_initialized_client(); + EXPECT_FALSE(rcl_client_is_valid(&client)); + rcl_reset_error(); + + // Check that a valid client is valid + client = rcl_get_zero_initialized_client(); + ret = rcl_client_init(&client, this->node_ptr, ts, topic_name, &default_client_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_client_is_valid(&client)); + ret = rcl_client_init(&client, this->node_ptr, ts, topic_name, &default_client_options); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << rcl_get_error_string().str; + ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing an invalid (uninitialized) node in init. + client = rcl_get_zero_initialized_client(); + rcl_node_t invalid_node = rcl_get_zero_initialized_node(); + ret = rcl_client_init(&client, &invalid_node, ts, topic_name, &default_client_options); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing null for the type support in init. + client = rcl_get_zero_initialized_client(); + ret = rcl_client_init( + &client, this->node_ptr, nullptr, topic_name, &default_client_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing null for the topic name in init. + client = rcl_get_zero_initialized_client(); + ret = rcl_client_init(&client, this->node_ptr, ts, nullptr, &default_client_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing null for the options in init. + client = rcl_get_zero_initialized_client(); + ret = rcl_client_init(&client, this->node_ptr, ts, topic_name, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing options with an invalid allocate in allocator with init. + client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options_with_invalid_allocator; + client_options_with_invalid_allocator = rcl_client_get_default_options(); + client_options_with_invalid_allocator.allocator.allocate = nullptr; + ret = rcl_client_init( + &client, this->node_ptr, ts, topic_name, &client_options_with_invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing options with an invalid deallocate in allocator with init. + client = rcl_get_zero_initialized_client(); + client_options_with_invalid_allocator = rcl_client_get_default_options(); + client_options_with_invalid_allocator.allocator.deallocate = nullptr; + ret = rcl_client_init( + &client, this->node_ptr, ts, topic_name, &client_options_with_invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // An allocator with an invalid realloc will probably work (so we will not test it). + + // Try passing options with a failing allocator with init. + client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options_with_failing_allocator; + client_options_with_failing_allocator = rcl_client_get_default_options(); + client_options_with_failing_allocator.allocator.allocate = failing_malloc; + client_options_with_failing_allocator.allocator.reallocate = failing_realloc; + ret = rcl_client_init( + &client, this->node_ptr, ts, topic_name, &client_options_with_failing_allocator); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +/* Passing bad/invalid arguments to the functions + */ +TEST_F(TestClientFixture, test_client_bad_arguments) { + rcl_client_t client = rcl_get_zero_initialized_client(); + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + rcl_client_options_t default_client_options = rcl_client_get_default_options(); + + EXPECT_EQ( + RCL_RET_SERVICE_NAME_INVALID, rcl_client_init( + &client, this->node_ptr, ts, + "invalid name", &default_client_options)) << rcl_get_error_string().str; + + EXPECT_EQ(RCL_RET_NODE_INVALID, rcl_client_fini(&client, nullptr)); + rcl_node_t not_valid_node = rcl_get_zero_initialized_node(); + EXPECT_EQ(RCL_RET_NODE_INVALID, rcl_client_fini(&client, ¬_valid_node)); + + rmw_service_info_t header; + int64_t sequence_number = 24; + test_msgs__srv__BasicTypes_Response client_response; + test_msgs__srv__BasicTypes_Request client_request; + test_msgs__srv__BasicTypes_Request__init(&client_request); + test_msgs__srv__BasicTypes_Response__init(&client_response); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__srv__BasicTypes_Response__fini(&client_response); + test_msgs__srv__BasicTypes_Request__fini(&client_request); + }); + + EXPECT_EQ(nullptr, rcl_client_get_rmw_handle(nullptr)); + EXPECT_EQ(nullptr, rcl_client_get_service_name(nullptr)); + EXPECT_EQ(nullptr, rcl_client_get_service_name(nullptr)); + EXPECT_EQ(nullptr, rcl_client_get_options(nullptr)); + EXPECT_EQ( + RCL_RET_CLIENT_INVALID, rcl_take_response_with_info( + nullptr, &header, &client_response)) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_CLIENT_INVALID, rcl_take_response( + nullptr, &(header.request_id), &client_response)) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_CLIENT_INVALID, rcl_send_request( + nullptr, &client_request, &sequence_number)) << rcl_get_error_string().str; + EXPECT_EQ(24, sequence_number); + EXPECT_EQ(nullptr, rcl_client_request_publisher_get_actual_qos(nullptr)); + EXPECT_EQ(nullptr, rcl_client_response_subscription_get_actual_qos(nullptr)); + + // Not init client + EXPECT_EQ(nullptr, rcl_client_get_rmw_handle(&client)); + EXPECT_EQ(nullptr, rcl_client_get_service_name(&client)); + EXPECT_EQ(nullptr, rcl_client_get_service_name(&client)); + EXPECT_EQ(nullptr, rcl_client_get_options(&client)); + EXPECT_EQ( + RCL_RET_CLIENT_INVALID, rcl_take_response_with_info( + &client, &header, &client_response)) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_CLIENT_INVALID, rcl_take_response( + &client, &(header.request_id), &client_response)) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_CLIENT_INVALID, rcl_send_request( + &client, &client_request, &sequence_number)) << rcl_get_error_string().str; + EXPECT_EQ(24, sequence_number); + EXPECT_EQ(nullptr, rcl_client_request_publisher_get_actual_qos(&client)); + EXPECT_EQ(nullptr, rcl_client_response_subscription_get_actual_qos(&client)); +} + +TEST_F(TestClientFixture, test_client_init_fini_maybe_fail) +{ + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + constexpr char topic_name[] = "chatter"; + rcl_client_options_t default_client_options = rcl_client_get_default_options(); + + RCUTILS_FAULT_INJECTION_TEST( + { + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_ret_t ret = rcl_client_init( + &client, this->node_ptr, ts, topic_name, &default_client_options); + if (RCL_RET_OK == ret) { + EXPECT_TRUE(rcl_client_is_valid(&client)); + ret = rcl_client_fini(&client, this->node_ptr); + if (RCL_RET_OK != ret) { + // If fault injection caused fini to fail, we should try it again. + EXPECT_EQ(RCL_RET_OK, rcl_client_fini(&client, this->node_ptr)); + } + } else { + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + }); +} diff --git a/test/rcl/test_common.cpp b/test/rcl/test_common.cpp new file mode 100644 index 0000000..f2e2dd8 --- /dev/null +++ b/test/rcl/test_common.cpp @@ -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. + +#include + +#include "rcl/rcl.h" +#include "./common.h" + +// This function is not part of the public API +TEST(TestCommonFunctionality, test_rmw_ret_to_rcl_ret) { + EXPECT_EQ(RCL_RET_OK, rcl_convert_rmw_ret_to_rcl_ret(RMW_RET_OK)); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_convert_rmw_ret_to_rcl_ret(RMW_RET_INVALID_ARGUMENT)); + EXPECT_EQ(RCL_RET_BAD_ALLOC, rcl_convert_rmw_ret_to_rcl_ret(RMW_RET_BAD_ALLOC)); + EXPECT_EQ(RCL_RET_UNSUPPORTED, rcl_convert_rmw_ret_to_rcl_ret(RMW_RET_UNSUPPORTED)); + EXPECT_EQ( + RCL_RET_NODE_NAME_NON_EXISTENT, + rcl_convert_rmw_ret_to_rcl_ret(RMW_RET_NODE_NAME_NON_EXISTENT)); + + // Default behavior + EXPECT_EQ(RCL_RET_ERROR, rcl_convert_rmw_ret_to_rcl_ret(RMW_RET_ERROR)); + EXPECT_EQ(RCL_RET_ERROR, rcl_convert_rmw_ret_to_rcl_ret(RMW_RET_TIMEOUT)); + EXPECT_EQ(RCL_RET_ERROR, rcl_convert_rmw_ret_to_rcl_ret(RMW_RET_INCORRECT_RMW_IMPLEMENTATION)); +} diff --git a/test/rcl/test_context.cpp b/test/rcl/test_context.cpp new file mode 100644 index 0000000..31e4100 --- /dev/null +++ b/test/rcl/test_context.cpp @@ -0,0 +1,187 @@ +// 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 "osrf_testing_tools_cpp/memory_tools/gtest_quickstart.hpp" +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/context.h" +#include "rcl/error_handling.h" +#include "rcl/init.h" + +#include "rmw/rmw.h" + +#include "../mocking_utils/patch.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestContextFixture, RMW_IMPLEMENTATION) : public ::testing::Test {}; + +// Test the rcl_context_t's normal function. +// Note: that init/fini are tested in test_init.cpp. +TEST_F(CLASSNAME(TestContextFixture, RMW_IMPLEMENTATION), nominal) { + osrf_testing_tools_cpp::memory_tools::ScopedQuickstartGtest scoped_quickstart_gtest; + + // This prevents memory allocations when setting error states in the future. + rcl_ret_t ret = rcl_initialize_error_handling_thread_local_storage(rcl_get_default_allocator()); + ASSERT_EQ(ret, RCL_RET_OK); + + // initialization with rcl_init + rcl_context_t context = rcl_get_zero_initialized_context(); + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(ret, RCL_RET_OK); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(ret, RCL_RET_OK); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_shutdown(&context); + EXPECT_EQ(ret, RCL_RET_OK); + ret = rcl_context_fini(&context); + EXPECT_EQ(ret, RCL_RET_OK); + }); + + // test rcl_context_get_init_options + const rcl_init_options_t * init_options_ptr; + EXPECT_NO_MEMORY_OPERATIONS( + { + init_options_ptr = rcl_context_get_init_options(nullptr); + }); + EXPECT_EQ(init_options_ptr, nullptr); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_NO_MEMORY_OPERATIONS( + { + init_options_ptr = rcl_context_get_init_options(&context); + }); + EXPECT_NE(init_options_ptr, nullptr) << rcl_get_error_string().str; + rcl_reset_error(); + + // test rcl_context_get_instance_id + rcl_context_instance_id_t instance_id; + EXPECT_NO_MEMORY_OPERATIONS( + { + instance_id = rcl_context_get_instance_id(nullptr); + }); + EXPECT_EQ(instance_id, 0UL); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_NO_MEMORY_OPERATIONS( + { + instance_id = rcl_context_get_instance_id(&context); + }); + EXPECT_NE(instance_id, 0UL) << rcl_get_error_string().str; + rcl_reset_error(); + + // test rcl_context_get_domain_id + size_t domain_id; + + EXPECT_NO_MEMORY_OPERATIONS( + { + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_context_get_domain_id(&context, nullptr)); + }); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_NO_MEMORY_OPERATIONS( + { + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_context_get_domain_id(nullptr, &domain_id)); + }); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_NO_MEMORY_OPERATIONS( + { + EXPECT_EQ(RCL_RET_OK, rcl_context_get_domain_id(&context, &domain_id)); + }); + + // test rcl_context_is_valid + bool is_valid; + EXPECT_NO_MEMORY_OPERATIONS( + { + is_valid = rcl_context_is_valid(nullptr); + }); + EXPECT_FALSE(is_valid); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_NO_MEMORY_OPERATIONS( + { + is_valid = rcl_context_is_valid(&context); + }); + EXPECT_TRUE(is_valid) << rcl_get_error_string().str; + rcl_reset_error(); + + // test rcl_context_get_rmw_context + rmw_context_t * rmw_context_ptr; + EXPECT_NO_MEMORY_OPERATIONS( + { + rmw_context_ptr = rcl_context_get_rmw_context(nullptr); + }); + EXPECT_EQ(rmw_context_ptr, nullptr); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_NO_MEMORY_OPERATIONS( + { + rmw_context_ptr = rcl_context_get_rmw_context(&context); + }); + EXPECT_NE(rmw_context_ptr, nullptr) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_init_options_fini(&init_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +TEST_F(CLASSNAME(TestContextFixture, RMW_IMPLEMENTATION), bad_fini) { + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_context_fini(nullptr)); + rcl_reset_error(); + + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + + rcl_context_t context = rcl_get_zero_initialized_context(); + + ret = rcl_context_fini(&context); + EXPECT_EQ(RCL_RET_OK, ret); + + ret = rcl_init(0, nullptr, &init_options, &context); + EXPECT_EQ(RCL_RET_OK, ret); + + ret = rcl_context_fini(&context); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + rcl_reset_error(); + + ret = rcl_shutdown(&context); + EXPECT_EQ(ret, RCL_RET_OK); + + { + auto mock = mocking_utils::inject_on_return( + "lib:rcl", rmw_context_fini, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_context_fini(&context)); + rcl_reset_error(); + } +} diff --git a/test/rcl/test_count_matched.cpp b/test/rcl/test_count_matched.cpp new file mode 100644 index 0000000..b0baaa8 --- /dev/null +++ b/test/rcl/test_count_matched.cpp @@ -0,0 +1,266 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#include + +#include +#include +#include + +#include "rcl/rcl.h" +#include "rcl/publisher.h" +#include "rcl/subscription.h" + +#include "rcutils/logging_macros.h" + +#include "test_msgs/msg/basic_types.h" +#include "test_msgs/srv/basic_types.h" + +#include "rcl/error_handling.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +void check_state( + rcl_wait_set_t * wait_set_ptr, + rcl_publisher_t * publisher, + rcl_subscription_t * subscriber, + const rcl_guard_condition_t * graph_guard_condition, + size_t expected_subscriber_count, + size_t expected_publisher_count, + size_t number_of_tries) +{ + size_t subscriber_count = -1; + size_t publisher_count = -1; + + rcl_ret_t ret; + + for (size_t i = 0; i < number_of_tries; ++i) { + if (publisher) { + ret = rcl_publisher_get_subscription_count(publisher, &subscriber_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + + if (subscriber) { + ret = rcl_subscription_get_publisher_count(subscriber, &publisher_count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + + if ( + expected_publisher_count == publisher_count && + expected_subscriber_count == subscriber_count) + { + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, " state correct!"); + break; + } + + if ((i + 1) == number_of_tries) { + // Don't wait for the graph to change on the last loop because we won't check again. + continue; + } + + ret = rcl_wait_set_clear(wait_set_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_guard_condition(wait_set_ptr, graph_guard_condition, NULL); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200); + RCUTILS_LOG_INFO_NAMED( + ROS_PACKAGE_NAME, + " state wrong, waiting up to '%s' nanoseconds for graph changes... ", + std::to_string(time_to_sleep.count()).c_str()); + ret = rcl_wait(wait_set_ptr, time_to_sleep.count()); + if (ret == RCL_RET_TIMEOUT) { + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "timeout"); + continue; + } + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "change occurred"); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + EXPECT_EQ(expected_publisher_count, publisher_count); + EXPECT_EQ(expected_subscriber_count, subscriber_count); +} + +class CLASSNAME (TestCountFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_node_t * node_ptr; + rcl_context_t * context_ptr; + rcl_wait_set_t * wait_set_ptr; + void SetUp() + { + rcl_ret_t ret; + rcl_node_options_t node_options = rcl_node_get_default_options(); + + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_init_options_fini(&init_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "test_count_node"; + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->wait_set_ptr = new rcl_wait_set_t; + *this->wait_set_ptr = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init( + this->wait_set_ptr, 0, 1, 0, 0, 0, 0, this->context_ptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret; + + ret = rcl_wait_set_fini(this->wait_set_ptr); + delete this->wait_set_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +TEST_F(CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), test_count_matched_functions) { + std::string topic_name("/test_count_matched_functions__"); + rcl_ret_t ret; + + rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); + auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + ret = rcl_publisher_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + const rcl_guard_condition_t * graph_guard_condition = + rcl_node_get_graph_guard_condition(this->node_ptr); + + check_state(wait_set_ptr, &pub, nullptr, graph_guard_condition, 0, -1, 9); + + rcl_subscription_t sub = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&sub, this->node_ptr, ts, topic_name.c_str(), &sub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 1, 1, 9); + + rcl_subscription_t sub2 = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t sub2_ops = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&sub2, this->node_ptr, ts, topic_name.c_str(), &sub2_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 2, 1, 9); + check_state(wait_set_ptr, &pub, &sub2, graph_guard_condition, 2, 1, 9); + + ret = rcl_publisher_fini(&pub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + check_state(wait_set_ptr, nullptr, &sub, graph_guard_condition, -1, 0, 9); + check_state(wait_set_ptr, nullptr, &sub2, graph_guard_condition, -1, 0, 9); + + ret = rcl_subscription_fini(&sub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_subscription_fini(&sub2, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST_F( + CLASSNAME(TestCountFixture, RMW_IMPLEMENTATION), + test_count_matched_functions_mismatched_qos) { + std::string topic_name("/test_count_matched_functions_mismatched_qos__"); + rcl_ret_t ret; + + rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); + + rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); + pub_ops.qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + pub_ops.qos.depth = 10; + pub_ops.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + pub_ops.qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + pub_ops.qos.avoid_ros_namespace_conventions = false; + pub_ops.allocator = rcl_get_default_allocator(); + + auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + ret = rcl_publisher_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_ops); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + const rcl_guard_condition_t * graph_guard_condition = + rcl_node_get_graph_guard_condition(this->node_ptr); + + check_state(wait_set_ptr, &pub, nullptr, graph_guard_condition, 0, -1, 9); + + rcl_subscription_t sub = rcl_get_zero_initialized_subscription(); + + rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options(); + sub_ops.qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + sub_ops.qos.depth = 10; + sub_ops.qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + sub_ops.qos.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + sub_ops.qos.avoid_ros_namespace_conventions = false; + sub_ops.allocator = rcl_get_default_allocator(); + + ret = rcl_subscription_init(&sub, this->node_ptr, ts, topic_name.c_str(), &sub_ops); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Expect that no publishers or subscribers should be matched due to qos. + check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 0, 0, 9); + + rcl_subscription_t sub2 = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t sub2_ops = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&sub2, this->node_ptr, ts, topic_name.c_str(), &sub2_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Even multiple subscribers should not match + check_state(wait_set_ptr, &pub, &sub, graph_guard_condition, 0, 0, 9); + check_state(wait_set_ptr, &pub, &sub2, graph_guard_condition, 0, 0, 9); + + ret = rcl_subscription_fini(&sub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_subscription_fini(&sub2, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_publisher_fini(&pub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} diff --git a/test/rcl/test_domain_id.cpp b/test/rcl/test_domain_id.cpp new file mode 100644 index 0000000..55a0413 --- /dev/null +++ b/test/rcl/test_domain_id.cpp @@ -0,0 +1,64 @@ +// 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 "rcl/rcl.h" + +#include "rcl/domain_id.h" +#include "rcl/error_handling.h" +#include "rcutils/env.h" + +#include "../mocking_utils/patch.hpp" + +TEST(TestGetDomainId, test_nominal) { + ASSERT_TRUE(rcutils_set_env("ROS_DOMAIN_ID", "42")); + size_t domain_id = RCL_DEFAULT_DOMAIN_ID; + EXPECT_EQ(RCL_RET_OK, rcl_get_default_domain_id(&domain_id)); + EXPECT_EQ(42u, domain_id); + + ASSERT_TRUE(rcutils_set_env("ROS_DOMAIN_ID", "")); + domain_id = RCL_DEFAULT_DOMAIN_ID; + EXPECT_EQ(RCL_RET_OK, rcl_get_default_domain_id(&domain_id)); + EXPECT_EQ(RCL_DEFAULT_DOMAIN_ID, domain_id); + + ASSERT_TRUE(rcutils_set_env("ROS_DOMAIN_ID", "0000")); + domain_id = RCL_DEFAULT_DOMAIN_ID; + EXPECT_EQ(RCL_RET_OK, rcl_get_default_domain_id(&domain_id)); + EXPECT_EQ(0u, domain_id); + + ASSERT_TRUE(rcutils_set_env("ROS_DOMAIN_ID", "0 not really")); + domain_id = RCL_DEFAULT_DOMAIN_ID; + EXPECT_EQ(RCL_RET_ERROR, rcl_get_default_domain_id(&domain_id)); + rcl_reset_error(); + EXPECT_EQ(RCL_DEFAULT_DOMAIN_ID, domain_id); + + ASSERT_TRUE(rcutils_set_env("ROS_DOMAIN_ID", "998446744073709551615")); + domain_id = RCL_DEFAULT_DOMAIN_ID; + EXPECT_EQ(RCL_RET_ERROR, rcl_get_default_domain_id(&domain_id)); + rcl_reset_error(); + EXPECT_EQ(RCL_DEFAULT_DOMAIN_ID, domain_id); + + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_get_default_domain_id(nullptr)); +} + +TEST(TestGetDomainId, test_mock_get_default_domain_id) { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_get_env, "argument env_name is null"); + size_t domain_id = RCL_DEFAULT_DOMAIN_ID; + EXPECT_EQ(RCL_RET_ERROR, rcl_get_default_domain_id(&domain_id)); + EXPECT_EQ(RCL_DEFAULT_DOMAIN_ID, domain_id); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} diff --git a/test/rcl/test_events.cpp b/test/rcl/test_events.cpp new file mode 100644 index 0000000..3489a0d --- /dev/null +++ b/test/rcl/test_events.cpp @@ -0,0 +1,849 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include "rcl/rcl.h" +#include "rcl/subscription.h" +#include "rcl/error_handling.h" +#include "rmw/incompatible_qos_events_statuses.h" +#include "rmw/event.h" + +#include "test_msgs/msg/strings.h" +#include "rosidl_runtime_c/string_functions.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" + +#include "./event_impl.h" + +using namespace std::chrono_literals; +using std::chrono::seconds; +using std::chrono::duration_cast; + +constexpr seconds LIVELINESS_LEASE_DURATION_IN_S = 1s; +constexpr seconds DEADLINE_PERIOD_IN_S = 2s; +constexpr seconds MAX_WAIT_PER_TESTCASE = 10s; + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +#define EXPECT_OK(varname) EXPECT_EQ(varname, RCL_RET_OK) << rcl_get_error_string().str + +struct TestIncompatibleQosEventParams +{ + std::string testcase_name; + rmw_qos_policy_kind_t qos_policy_kind; + rmw_qos_profile_t publisher_qos_profile; + rmw_qos_profile_t subscription_qos_profile; + std::string error_msg; +}; + +class CLASSNAME (TestEventFixture, RMW_IMPLEMENTATION) + : public ::testing::TestWithParam +{ +public: + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "test_event_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + } + + rcl_ret_t setup_publisher(const rmw_qos_profile_t qos_profile) + { + // init publisher + publisher = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + publisher_options.qos = qos_profile; + return rcl_publisher_init( + &publisher, + this->node_ptr, + this->ts, + this->topic, + &publisher_options); + } + + rcl_ret_t setup_subscriber(const rmw_qos_profile_t qos_profile) + { + // init publisher + subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + subscription_options.qos = qos_profile; + return rcl_subscription_init( + &subscription, + this->node_ptr, + this->ts, + this->topic, + &subscription_options); + } + + void setup_publisher_subscriber( + const rmw_qos_profile_t pub_qos_profile, + const rmw_qos_profile_t sub_qos_profile) + { + rcl_ret_t ret; + + // init publisher + ret = setup_publisher(pub_qos_profile); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + // init subscription + ret = setup_subscriber(sub_qos_profile); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + + void setup_publisher_subscriber_events( + const rcl_publisher_event_type_t & pub_event_type, + const rcl_subscription_event_type_t & sub_event_type) + { + rcl_ret_t ret; + + // init publisher events + publisher_event = rcl_get_zero_initialized_event(); + ret = rcl_publisher_event_init(&publisher_event, &publisher, pub_event_type); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + // init subscription event + subscription_event = rcl_get_zero_initialized_event(); + ret = rcl_subscription_event_init(&subscription_event, &subscription, sub_event_type); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + + void setup_publisher_subscriber_and_events_and_assert_discovery( + const rcl_publisher_event_type_t & pub_event_type, + const rcl_subscription_event_type_t & sub_event_type) + { + setup_publisher_subscriber(default_qos_profile, default_qos_profile); + setup_publisher_subscriber_events(pub_event_type, sub_event_type); + + rcl_ret_t ret; + // wait for discovery, time out after 10s + static const size_t max_iterations = 1000; + static const auto wait_period = 10ms; + bool subscribe_success = false; + for (size_t i = 0; i < max_iterations; ++i) { + size_t subscription_count = 0; + size_t publisher_count = 0; + ret = rcl_subscription_get_publisher_count(&subscription, &publisher_count); + EXPECT_OK(ret); + ret = rcl_publisher_get_subscription_count(&publisher, &subscription_count); + EXPECT_OK(ret); + if (subscription_count && publisher_count) { + subscribe_success = true; + break; + } + std::this_thread::sleep_for(wait_period); + } + ASSERT_TRUE(subscribe_success) << "Publisher/Subscription discovery timed out"; + } + + void tear_down_publisher_subscriber() + { + rcl_ret_t ret; + + ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + + void tear_down_publisher_subscriber_events() + { + rcl_ret_t ret; + + ret = rcl_event_fini(&subscription_event); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_event_fini(&publisher_event); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret; + + ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + + struct PrintToStringParamName + { + template + std::string operator()(const ::testing::TestParamInfo & info) const + { + const auto & test_params = static_cast(info.param); + return test_params.testcase_name; + } + }; + + static const rmw_qos_profile_t default_qos_profile; + +protected: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + + rcl_publisher_t publisher; + rcl_event_t publisher_event; + rcl_subscription_t subscription; + rcl_event_t subscription_event; + const char * topic = "rcl_test_publisher_subscription_events"; + const rosidl_message_type_support_t * ts; +}; + +using TestEventFixture = CLASSNAME(TestEventFixture, RMW_IMPLEMENTATION); + +const rmw_qos_profile_t TestEventFixture::default_qos_profile = { + RMW_QOS_POLICY_HISTORY_KEEP_LAST, // history + 0, // depth + RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, // reliability + RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT, // durability + {DEADLINE_PERIOD_IN_S.count(), 0}, // deadline + {0, 0}, // lifespan + RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC, // liveliness + {LIVELINESS_LEASE_DURATION_IN_S.count(), 0}, // liveliness_lease_duration + false // avoid_ros_namespace_conventions +}; + +rcl_ret_t +wait_for_msgs_and_events( + rcl_context_t * context, + rcl_subscription_t * subscription, + rcl_event_t * subscription_event, + rcl_event_t * publisher_event, + bool * msg_ready, + bool * subscription_event_ready, + bool * publisher_event_ready, + seconds period = 1s) +{ + *msg_ready = false; + *subscription_event_ready = false; + *publisher_event_ready = false; + + int num_subscriptions = (nullptr == subscription ? 0 : 1); + int num_events = (nullptr == subscription_event ? 0 : 1) + (nullptr == publisher_event ? 0 : 1); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = rcl_wait_set_init( + &wait_set, + num_subscriptions, 0, 0, 0, 0, num_events, + context, + rcl_get_default_allocator()); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + }); + + ret = rcl_wait_set_clear(&wait_set); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + if (nullptr != subscription) { + ret = rcl_wait_set_add_subscription(&wait_set, subscription, NULL); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + if (nullptr != subscription_event) { + ret = rcl_wait_set_add_event(&wait_set, subscription_event, NULL); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + if (nullptr != publisher_event) { + ret = rcl_wait_set_add_event(&wait_set, publisher_event, NULL); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + + ret = rcl_wait(&wait_set, RCL_S_TO_NS(period.count())); + if (ret == RCL_RET_TIMEOUT) { + return ret; + } + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + for (size_t i = 0; i < wait_set.size_of_subscriptions; ++i) { + if (wait_set.subscriptions[i] && wait_set.subscriptions[i] == subscription) { + *msg_ready = true; + } + } + for (size_t i = 0; i < wait_set.size_of_events; ++i) { + if (nullptr != wait_set.events[i]) { + if (wait_set.events[i] == subscription_event) { + *subscription_event_ready = true; + } else if (wait_set.events[i] == publisher_event) { + *publisher_event_ready = true; + } + } + } + ret = rcl_wait_set_fini(&wait_set); + EXPECT_OK(ret); + return ret; +} + +/// Conditional function for determining when the wait_for_msgs_and_events loop is complete. +/** + * Conditional function for determining when the wait_for_msgs_and_events loop is complete. + * + * \param msg_persist_ready true if a msg has ever been received + * \param subscription_persist_readytrue if a subscription has been received + * \param publisher_persist_ready true if a pulbisher event has been received + * \return true when the desired conditions are met + */ +using WaitConditionPredicate = std::function< + bool ( + const bool & msg_persist_ready, + const bool & subscription_persist_ready, + const bool & publisher_persist_ready + )>; + +// Wait for msgs and events until all conditions are met or a timeout has occured. +rcl_ret_t +conditional_wait_for_msgs_and_events( + rcl_context_t * context, + seconds timeout, + const WaitConditionPredicate & events_ready, + rcl_subscription_t * subscription, + rcl_event_t * subscription_event, + rcl_event_t * publisher_event, + bool * msg_persist_ready, + bool * subscription_persist_ready, + bool * publisher_persist_ready) +{ + *msg_persist_ready = false; + *subscription_persist_ready = false; + *publisher_persist_ready = false; + + auto start_time = std::chrono::system_clock::now(); + while (std::chrono::system_clock::now() - start_time < timeout) { + bool msg_ready, subscription_event_ready, publisher_event_ready; + + // wait for msg and events + rcl_ret_t ret = wait_for_msgs_and_events( + context, subscription, subscription_event, publisher_event, + &msg_ready, &subscription_event_ready, &publisher_event_ready); + if (RCL_RET_OK != ret) { + continue; + } + + *msg_persist_ready |= msg_ready; + *subscription_persist_ready |= subscription_event_ready; + *publisher_persist_ready |= publisher_event_ready; + if (events_ready(*msg_persist_ready, *subscription_persist_ready, *publisher_persist_ready)) { + return RCL_RET_OK; + } + } + return RCL_RET_TIMEOUT; +} + +/* + * Basic test of publisher and subscriber deadline events, with first message sent before deadline + */ +TEST_F(TestEventFixture, test_pubsub_no_deadline_missed) +{ + setup_publisher_subscriber_and_events_and_assert_discovery( + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + rcl_ret_t ret; + + // publish message to topic + const char * test_string = "testing"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + + // wait for msg and events + bool msg_ready, subscription_event_ready, publisher_event_ready; + rcl_ret_t wait_res = wait_for_msgs_and_events( + context_ptr, &subscription, &subscription_event, &publisher_event, + &msg_ready, &subscription_event_ready, &publisher_event_ready, DEADLINE_PERIOD_IN_S); + EXPECT_EQ(wait_res, RCL_RET_OK); + + // test that the message published to topic is as expected + EXPECT_TRUE(msg_ready); + if (msg_ready) { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(std::string(msg.string_value.data, msg.string_value.size), std::string(test_string)); + } + + // test subscriber/datareader deadline missed status + EXPECT_FALSE(subscription_event_ready); + { + rmw_requested_deadline_missed_status_t deadline_status; + ret = rcl_take_event(&subscription_event, &deadline_status); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(deadline_status.total_count, 0); + EXPECT_EQ(deadline_status.total_count_change, 0); + } + + // test publisher/datawriter deadline missed status + EXPECT_FALSE(publisher_event_ready); + { + rmw_offered_deadline_missed_status_t deadline_status; + ret = rcl_take_event(&publisher_event, &deadline_status); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(deadline_status.total_count, 0); + EXPECT_EQ(deadline_status.total_count_change, 0); + } + + // clean up + tear_down_publisher_subscriber_events(); + tear_down_publisher_subscriber(); +} + +/* + * Basic test of publisher and subscriber deadline events, with first message sent after deadline + */ +TEST_F(TestEventFixture, test_pubsub_deadline_missed) +{ + setup_publisher_subscriber_and_events_and_assert_discovery( + RCL_PUBLISHER_OFFERED_DEADLINE_MISSED, + RCL_SUBSCRIPTION_REQUESTED_DEADLINE_MISSED); + rcl_ret_t ret; + + // publish message to topic + const char * test_string = "testing"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + + WaitConditionPredicate all_ready = []( + const bool & msg_persist_ready, + const bool & subscription_persist_ready, + const bool & publisher_persist_ready) { + return msg_persist_ready && subscription_persist_ready && publisher_persist_ready; + }; + bool msg_persist_ready, subscription_persist_ready, publisher_persist_ready; + rcl_ret_t wait_res = conditional_wait_for_msgs_and_events( + context_ptr, MAX_WAIT_PER_TESTCASE, all_ready, + &subscription, &subscription_event, &publisher_event, + &msg_persist_ready, &subscription_persist_ready, &publisher_persist_ready); + EXPECT_EQ(wait_res, RCL_RET_OK); + + // test that the message published to topic is as expected + EXPECT_TRUE(msg_persist_ready); + if (msg_persist_ready) { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ( + std::string(msg.string_value.data, msg.string_value.size), + std::string(test_string)); + } + + // test subscriber/datareader deadline missed status + EXPECT_TRUE(subscription_persist_ready); + if (subscription_persist_ready) { + rmw_requested_deadline_missed_status_t requested_deadline_status; + ret = rcl_take_event(&subscription_event, &requested_deadline_status); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(requested_deadline_status.total_count, 1); + EXPECT_EQ(requested_deadline_status.total_count_change, 1); + } + + // test publisher/datawriter deadline missed status + EXPECT_TRUE(publisher_persist_ready); + if (publisher_persist_ready) { + rmw_offered_deadline_missed_status_t offered_deadline_status; + ret = rcl_take_event(&publisher_event, &offered_deadline_status); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(offered_deadline_status.total_count, 1); + EXPECT_EQ(offered_deadline_status.total_count_change, 1); + } + + // clean up + tear_down_publisher_subscriber_events(); + tear_down_publisher_subscriber(); +} + +/* + * Basic test of publisher and subscriber liveliness events, with publisher killed + */ +TEST_F(TestEventFixture, test_pubsub_liveliness_kill_pub) +{ + setup_publisher_subscriber_and_events_and_assert_discovery( + RCL_PUBLISHER_LIVELINESS_LOST, + RCL_SUBSCRIPTION_LIVELINESS_CHANGED); + rcl_ret_t ret; + + // publish message to topic + const char * test_string = "testing"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + + std::this_thread::sleep_for(2 * LIVELINESS_LEASE_DURATION_IN_S); + + WaitConditionPredicate all_ready = []( + const bool & msg_persist_ready, + const bool & subscription_persist_ready, + const bool & publisher_persist_ready) { + return msg_persist_ready && subscription_persist_ready && publisher_persist_ready; + }; + bool msg_persist_ready, subscription_persist_ready, publisher_persist_ready; + rcl_ret_t wait_res = conditional_wait_for_msgs_and_events( + context_ptr, MAX_WAIT_PER_TESTCASE, all_ready, + &subscription, &subscription_event, &publisher_event, + &msg_persist_ready, &subscription_persist_ready, &publisher_persist_ready); + EXPECT_EQ(wait_res, RCL_RET_OK); + + // test that the message published to topic is as expected + EXPECT_TRUE(msg_persist_ready); + if (msg_persist_ready) { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ( + std::string(msg.string_value.data, msg.string_value.size), + std::string(test_string)); + } + + // test subscriber/datareader liveliness changed status + EXPECT_TRUE(subscription_persist_ready); + if (subscription_persist_ready) { + rmw_liveliness_changed_status_t liveliness_status; + ret = rcl_take_event(&subscription_event, &liveliness_status); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(liveliness_status.alive_count, 0); + EXPECT_EQ(liveliness_status.alive_count_change, 0); + EXPECT_EQ(liveliness_status.not_alive_count, 1); + EXPECT_EQ(liveliness_status.not_alive_count_change, 1); + } + + // test that the killed publisher/datawriter has no active events + EXPECT_TRUE(publisher_persist_ready); + if (publisher_persist_ready) { + rmw_liveliness_lost_status_t liveliness_status; + ret = rcl_take_event(&publisher_event, &liveliness_status); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(liveliness_status.total_count, 1); + EXPECT_EQ(liveliness_status.total_count_change, 1); + } + + // clean up + tear_down_publisher_subscriber_events(); + tear_down_publisher_subscriber(); +} + +/* + * Basic test of publisher and subscriber incompatible qos callback events. + */ +TEST_P(TestEventFixture, test_pubsub_incompatible_qos) +{ + const auto & input = GetParam(); + const auto & qos_policy_kind = input.qos_policy_kind; + const auto & publisher_qos_profile = input.publisher_qos_profile; + const auto & subscription_qos_profile = input.subscription_qos_profile; + const auto & error_msg = input.error_msg; + + setup_publisher_subscriber(publisher_qos_profile, subscription_qos_profile); + setup_publisher_subscriber_events( + RCL_PUBLISHER_OFFERED_INCOMPATIBLE_QOS, + RCL_SUBSCRIPTION_REQUESTED_INCOMPATIBLE_QOS); + + WaitConditionPredicate events_ready = []( + const bool & /*msg_persist_ready*/, + const bool & subscription_persist_ready, + const bool & publisher_persist_ready) { + return subscription_persist_ready && publisher_persist_ready; + }; + bool msg_persist_ready, subscription_persist_ready, publisher_persist_ready; + rcl_ret_t wait_res = conditional_wait_for_msgs_and_events( + context_ptr, MAX_WAIT_PER_TESTCASE, events_ready, + &subscription, &subscription_event, &publisher_event, + &msg_persist_ready, &subscription_persist_ready, &publisher_persist_ready); + EXPECT_EQ(wait_res, RCL_RET_OK); + + // test that the subscriber/datareader discovered an incompatible publisher/datawriter + EXPECT_TRUE(subscription_persist_ready); + if (subscription_persist_ready) { + rmw_requested_qos_incompatible_event_status_t requested_incompatible_qos_status; + rcl_ret_t ret = rcl_take_event(&subscription_event, &requested_incompatible_qos_status); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + const auto & pub_total_count = requested_incompatible_qos_status.total_count; + const auto & pub_total_count_change = requested_incompatible_qos_status.total_count_change; + const auto & pub_last_policy_kind = requested_incompatible_qos_status.last_policy_kind; + if (pub_total_count != 0 && pub_total_count_change != 0 && pub_last_policy_kind != 0) { + EXPECT_EQ(pub_total_count, 1) << error_msg; + EXPECT_EQ(pub_total_count_change, 1) << error_msg; + EXPECT_EQ(pub_last_policy_kind, qos_policy_kind) << error_msg; + } else { + ADD_FAILURE() << "Subscription incompatible qos event timed out for: " << error_msg; + } + } + + // test that the publisher/datawriter discovered an incompatible subscription/datareader + EXPECT_TRUE(publisher_persist_ready); + if (publisher_persist_ready) { + rmw_offered_qos_incompatible_event_status_t offered_incompatible_qos_status; + rcl_ret_t ret = rcl_take_event(&publisher_event, &offered_incompatible_qos_status); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + const auto & sub_total_count = offered_incompatible_qos_status.total_count; + const auto & sub_total_count_change = offered_incompatible_qos_status.total_count_change; + const auto & sub_last_policy_kind = offered_incompatible_qos_status.last_policy_kind; + if (sub_total_count != 0 && sub_total_count_change != 0 && sub_last_policy_kind != 0) { + EXPECT_EQ(sub_total_count, 1) << error_msg; + EXPECT_EQ(sub_total_count_change, 1) << error_msg; + EXPECT_EQ(sub_last_policy_kind, qos_policy_kind) << error_msg; + } else { + ADD_FAILURE() << "Publisher incompatible qos event timed out for: " << error_msg; + } + } + + // clean up + tear_down_publisher_subscriber_events(); + tear_down_publisher_subscriber(); +} + +/* + * Passing bad param subscriber/publisher event ini + */ +TEST_F(TestEventFixture, test_bad_event_ini) +{ + setup_publisher_subscriber(default_qos_profile, default_qos_profile); + const rcl_subscription_event_type_t unknown_sub_type = (rcl_subscription_event_type_t) 5432; + const rcl_publisher_event_type_t unknown_pub_type = (rcl_publisher_event_type_t) 5432; + + publisher_event = rcl_get_zero_initialized_event(); + rcl_ret_t ret = rcl_publisher_event_init( + &publisher_event, + &publisher, + unknown_pub_type); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + + subscription_event = rcl_get_zero_initialized_event(); + ret = rcl_subscription_event_init( + &subscription_event, + &subscription, + unknown_sub_type); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT); + + tear_down_publisher_subscriber(); +} + +/* + * Test cases for the event_is_valid function + */ +TEST_F(TestEventFixture, test_event_is_valid) +{ + EXPECT_FALSE(rcl_event_is_valid(nullptr)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + setup_publisher_subscriber(default_qos_profile, default_qos_profile); + rcl_event_t publisher_event_test = rcl_get_zero_initialized_event(); + EXPECT_FALSE(rcl_event_is_valid(&publisher_event_test)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + rcl_ret_t ret = rcl_publisher_event_init( + &publisher_event_test, &publisher, RCL_PUBLISHER_OFFERED_DEADLINE_MISSED); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_event_is_valid(&publisher_event_test)); + + rmw_event_type_t saved_event_type = publisher_event_test.impl->rmw_handle.event_type; + publisher_event_test.impl->rmw_handle.event_type = RMW_EVENT_INVALID; + EXPECT_FALSE(rcl_event_is_valid(&publisher_event_test)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + publisher_event_test.impl->rmw_handle.event_type = saved_event_type; + + rcl_allocator_t saved_alloc = publisher_event_test.impl->allocator; + rcl_allocator_t bad_alloc = rcutils_get_zero_initialized_allocator(); + publisher_event_test.impl->allocator = bad_alloc; + EXPECT_FALSE(rcl_event_is_valid(&publisher_event_test)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + publisher_event_test.impl->allocator = saved_alloc; + + ret = rcl_event_fini(&publisher_event_test); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + tear_down_publisher_subscriber(); +} + +/* + * Test passing not init to take_event/get_handle + */ +TEST_F(TestEventFixture, test_event_is_invalid) { + // nullptr + rmw_offered_deadline_missed_status_t deadline_status; + EXPECT_EQ(RCL_RET_EVENT_INVALID, rcl_take_event(NULL, &deadline_status)); + EXPECT_EQ(NULL, rcl_event_get_rmw_handle(NULL)); + + // Zero Init, invalid + rcl_event_t publisher_event_test = rcl_get_zero_initialized_event(); + EXPECT_EQ(RCL_RET_EVENT_INVALID, rcl_take_event(&publisher_event_test, &deadline_status)); + EXPECT_EQ(NULL, rcl_event_get_rmw_handle(&publisher_event_test)); +} + +/* + * Basic test subscriber event message lost + */ +TEST_F(TestEventFixture, test_sub_message_lost_event) +{ + const rmw_qos_profile_t subscription_qos_profile = default_qos_profile; + + rcl_ret_t ret = setup_subscriber(subscription_qos_profile); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + subscription_event = rcl_get_zero_initialized_event(); + ret = rcl_subscription_event_init( + &subscription_event, + &subscription, + RCL_SUBSCRIPTION_MESSAGE_LOST); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_event_fini(&subscription_event); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + }); + + EXPECT_EQ(ret, RCL_RET_OK); + + // Can't reproduce reliably this event + // Test that take_event is able to read the configured event + rmw_message_lost_status_t message_lost_status; + ret = rcl_take_event(&subscription_event, &message_lost_status); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(message_lost_status.total_count, 0u); + EXPECT_EQ(message_lost_status.total_count_change, 0u); +} + +static +std::array +get_test_pubsub_incompatible_qos_inputs() +{ + // an array of tuples that holds the expected qos_policy_kind, publisher qos profile, + // subscription qos profile and the error message, in that order. + std::array inputs; + + // durability + inputs[0].testcase_name = "IncompatibleQoS_Durability"; + inputs[0].qos_policy_kind = RMW_QOS_POLICY_DURABILITY; + inputs[0].publisher_qos_profile = TestEventFixture::default_qos_profile; + inputs[0].publisher_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + inputs[0].subscription_qos_profile = TestEventFixture::default_qos_profile; + inputs[0].subscription_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + inputs[0].error_msg = "Incompatible qos durability"; + + // deadline + inputs[1].testcase_name = "IncompatibleQoS_Deadline"; + inputs[1].qos_policy_kind = RMW_QOS_POLICY_DEADLINE; + inputs[1].publisher_qos_profile = TestEventFixture::default_qos_profile; + inputs[1].publisher_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count() + 5, 0}; + inputs[1].subscription_qos_profile = TestEventFixture::default_qos_profile; + inputs[1].subscription_qos_profile.deadline = {DEADLINE_PERIOD_IN_S.count(), 0}; + inputs[1].error_msg = "Incompatible qos deadline"; + + // liveliness + inputs[2].testcase_name = "IncompatibleQoS_LivelinessPolicy"; + inputs[2].qos_policy_kind = RMW_QOS_POLICY_LIVELINESS; + inputs[2].publisher_qos_profile = TestEventFixture::default_qos_profile; + inputs[2].publisher_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + inputs[2].subscription_qos_profile = TestEventFixture::default_qos_profile; + inputs[2].subscription_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + inputs[2].error_msg = "Incompatible qos liveliness policy"; + + // liveliness lease duration + inputs[3].testcase_name = "IncompatibleQoS_LivelinessLeaseDuration"; + inputs[3].qos_policy_kind = RMW_QOS_POLICY_LIVELINESS; + inputs[3].publisher_qos_profile = TestEventFixture::default_qos_profile; + inputs[3].publisher_qos_profile.liveliness_lease_duration = {DEADLINE_PERIOD_IN_S.count() + 5, 0}; + inputs[3].subscription_qos_profile = TestEventFixture::default_qos_profile; + inputs[3].subscription_qos_profile.liveliness_lease_duration = {DEADLINE_PERIOD_IN_S.count(), 0}; + inputs[3].error_msg = "Incompatible qos liveliness lease duration"; + + // reliability + inputs[4].testcase_name = "IncompatibleQoS_Reliability"; + inputs[4].qos_policy_kind = RMW_QOS_POLICY_RELIABILITY; + inputs[4].publisher_qos_profile = TestEventFixture::default_qos_profile; + inputs[4].publisher_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + inputs[4].subscription_qos_profile = TestEventFixture::default_qos_profile; + inputs[4].subscription_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + inputs[4].error_msg = "Incompatible qos reliability"; + + return inputs; +} + +INSTANTIATE_TEST_SUITE_P( + TestPubSubIncompatibilityWithDifferentQosSettings, + TestEventFixture, + ::testing::ValuesIn(get_test_pubsub_incompatible_qos_inputs()), + TestEventFixture::PrintToStringParamName()); diff --git a/test/rcl/test_expand_topic_name.cpp b/test/rcl/test_expand_topic_name.cpp new file mode 100644 index 0000000..8ec900a --- /dev/null +++ b/test/rcl/test_expand_topic_name.cpp @@ -0,0 +1,347 @@ +// 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 +#include +#include + +#include "rcutils/repl_str.h" +#include "rcutils/strdup.h" + +#include "rcl/expand_topic_name.h" + +#include "rcl/error_handling.h" + +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + +#include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" + +using namespace std::string_literals; + +TEST(test_expand_topic_name, normal) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcutils_string_map_t subs = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcu_ret = rcutils_string_map_init(&subs, 0, allocator); + ASSERT_EQ(RCUTILS_RET_OK, rcu_ret); + ret = rcl_get_default_topic_name_substitutions(&subs); + ASSERT_EQ(RCL_RET_OK, ret); + + // {node}/chatter example + { + const char * topic = "{node}/chatter"; + const char * ns = "/my_ns"; + const char * node = "my_node"; + std::string expected = std::string(ns) + "/" + node + "/chatter"; + char * expanded_topic; + ret = rcl_expand_topic_name(topic, node, ns, &subs, allocator, &expanded_topic); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ(expected.c_str(), expanded_topic); + allocator.deallocate(expanded_topic, allocator.state); + } + + ret = rcutils_string_map_fini(&subs); + ASSERT_EQ(RCL_RET_OK, ret); +} + +TEST(test_expand_topic_name, invalid_arguments) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcutils_string_map_t subs = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcu_ret = rcutils_string_map_init(&subs, 0, allocator); + ASSERT_EQ(RCUTILS_RET_OK, rcu_ret); + ret = rcl_get_default_topic_name_substitutions(&subs); + ASSERT_EQ(RCL_RET_OK, ret); + + const char * topic = "{node}/chatter"; + const char * ns = "/my_ns"; + const char * node = "my_node"; + char * expanded_topic; + + // pass null for topic string + { + ret = rcl_expand_topic_name(NULL, node, ns, &subs, allocator, &expanded_topic); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + } + + // pass null for node name + { + ret = rcl_expand_topic_name(topic, NULL, ns, &subs, allocator, &expanded_topic); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + } + + // pass null for node namespace + { + ret = rcl_expand_topic_name(topic, node, NULL, &subs, allocator, &expanded_topic); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + } + + // pass null for subs + { + ret = rcl_expand_topic_name(topic, node, ns, NULL, allocator, &expanded_topic); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + } + + // pass null for expanded_topic + { + ret = rcl_expand_topic_name(topic, node, ns, &subs, allocator, NULL); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + } + + // pass invalid topic + { + ret = rcl_expand_topic_name("white space", node, ns, &subs, allocator, &expanded_topic); + EXPECT_EQ(RCL_RET_TOPIC_NAME_INVALID, ret); + rcl_reset_error(); + } + + // pass invalid node name + { + ret = rcl_expand_topic_name(topic, "/invalid_node", ns, &subs, allocator, &expanded_topic); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAME, ret); + rcl_reset_error(); + } + + // pass invalid node namespace + { + ret = rcl_expand_topic_name(topic, node, "white space", &subs, allocator, &expanded_topic); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAMESPACE, ret); + rcl_reset_error(); + } + + // pass failing allocator + { + rcl_allocator_t bad_allocator = get_failing_allocator(); + EXPECT_EQ( + RCL_RET_BAD_ALLOC, + rcl_expand_topic_name("/absolute", node, ns, &subs, bad_allocator, &expanded_topic)); + EXPECT_STREQ(NULL, expanded_topic); + rcl_reset_error(); + } + + ret = rcutils_string_map_fini(&subs); + ASSERT_EQ(RCL_RET_OK, ret); +} + +// Define dummy comparison operators for rcutils_allocator_t type +// to use with the Mimick mocking library +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) + +TEST(test_expand_topic_name, internal_error) { + constexpr char node_name[] = "bar"; + constexpr char ns[] = "/foo"; + + rcutils_string_map_t subs = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t uret = rcutils_string_map_init(&subs, 0, rcutils_get_default_allocator()); + ASSERT_EQ(RCUTILS_RET_OK, uret) << rcutils_get_error_string().str; + rcl_ret_t ret = rcl_get_default_topic_name_substitutions(&subs); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_allocator_t allocator = rcl_get_default_allocator(); + char * expanded_topic_name = nullptr; + + { + constexpr char topic_name[] = "/test"; + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rmw_validate_node_name, "internal error", RMW_RET_ERROR); + ret = rcl_expand_topic_name( + topic_name, node_name, ns, &subs, allocator, &expanded_topic_name); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + + { + constexpr char topic_name[] = "/test"; + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rmw_validate_namespace, "internal error", RMW_RET_ERROR); + ret = rcl_expand_topic_name( + topic_name, node_name, ns, &subs, allocator, &expanded_topic_name); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + + { + constexpr char topic_name_with_valid_substitution[] = "{node}/test"; + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rcutils_strndup, "failed to allocate", nullptr); + ret = rcl_expand_topic_name( + topic_name_with_valid_substitution, node_name, ns, + &subs, allocator, &expanded_topic_name); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + constexpr char topic_name_with_unknown_substitution[] = "{unknown}/test"; + ret = rcl_expand_topic_name( + topic_name_with_unknown_substitution, node_name, ns, + &subs, allocator, &expanded_topic_name); + EXPECT_EQ(RCL_RET_UNKNOWN_SUBSTITUTION, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + + { + constexpr char topic_name[] = "{node}/test"; + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rcutils_repl_str, "failed to allocate", nullptr); + ret = rcl_expand_topic_name( + topic_name, node_name, ns, &subs, allocator, &expanded_topic_name); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + + { + constexpr char topic_name[] = "/test"; + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rcutils_strdup, "failed to allocate", nullptr); + ret = rcl_expand_topic_name( + topic_name, node_name, ns, &subs, allocator, &expanded_topic_name); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + + ret = rcutils_string_map_fini(&subs); + ASSERT_EQ(RCL_RET_OK, ret); +} + +TEST(test_expand_topic_name, various_valid_topics) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcutils_string_map_t subs = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcu_ret = rcutils_string_map_init(&subs, 0, allocator); + ASSERT_EQ(RCUTILS_RET_OK, rcu_ret); + ret = rcl_get_default_topic_name_substitutions(&subs); + ASSERT_EQ(RCL_RET_OK, ret); + + std::vector> topics_that_should_expand_to = { + // {"input_topic", "node_name", "/namespace", "expected result"}, + {"/chatter", "my_node", "/my_ns", "/chatter"}, + {"chatter", "my_node", "/my_ns", "/my_ns/chatter"}, + {"{node}/chatter", "my_node", "/my_ns", "/my_ns/my_node/chatter"}, + {"/{node}", "my_node", "/my_ns", "/my_node"}, + {"{node}", "my_node", "/my_ns", "/my_ns/my_node"}, + {"{ns}", "my_node", "/my_ns", "/my_ns"}, + {"{namespace}", "my_node", "/my_ns", "/my_ns"}, + {"{namespace}/{node}/chatter", "my_node", "/my_ns", "/my_ns/my_node/chatter"}, + + // this one will produce an invalid topic, but will pass + // the '//' should be caught by the rmw_validate_full_topic_name() function + {"/foo/{namespace}", "my_node", "/my_ns", "/foo//my_ns"}, + + // examples from the design doc: + // http://design.ros2.org/articles/topic_and_service_names.html + // the node constructor would make the "" namespace into "/" implicitly + {"ping", "my_node", "/", "/ping"}, + {"ping", "my_node", "/my_ns", "/my_ns/ping"}, + + {"/ping", "my_node", "/", "/ping"}, + {"/ping", "my_node", "/my_ns", "/ping"}, + + {"~", "my_node", "/", "/my_node"}, + {"~", "my_node", "/my_ns", "/my_ns/my_node"}, + + {"~/ping", "my_node", "/", "/my_node/ping"}, + {"~/ping", "my_node", "/my_ns", "/my_ns/my_node/ping"}, + }; + + for (const auto & inout : topics_that_should_expand_to) { + const char * topic = inout.at(0).c_str(); + const char * node = inout.at(1).c_str(); + const char * ns = inout.at(2).c_str(); + std::string expected = inout.at(3); + char * expanded_topic; + ret = rcl_expand_topic_name(topic, node, ns, &subs, allocator, &expanded_topic); + std::stringstream ss; + ss << + "^ While expanding '" << topic << + "' with node '" << node << + "' and namespace '" << ns << "'"; + EXPECT_EQ(RCL_RET_OK, ret) << + ss.str() << + ", it failed with '" << ret << "': " << rcl_get_error_string().str; + EXPECT_STREQ(expected.c_str(), expanded_topic) << ss.str() << " strings did not match.\n"; + allocator.deallocate(expanded_topic, allocator.state); + } + + ret = rcutils_string_map_fini(&subs); + ASSERT_EQ(RCL_RET_OK, ret); +} + +TEST(test_expand_topic_name, unknown_substitution) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcutils_string_map_t subs = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcu_ret = rcutils_string_map_init(&subs, 0, allocator); + ASSERT_EQ(RCUTILS_RET_OK, rcu_ret); + ret = rcl_get_default_topic_name_substitutions(&subs); + ASSERT_EQ(RCL_RET_OK, ret); + + { + const char * topic = "{doesnotexist}"; + const char * ns = "/my_ns"; + const char * node = "my_node"; + char * expanded_topic; + ret = rcl_expand_topic_name(topic, node, ns, &subs, allocator, &expanded_topic); + EXPECT_EQ(RCL_RET_UNKNOWN_SUBSTITUTION, ret); + rcl_reset_error(); + EXPECT_EQ(NULL, expanded_topic); + allocator.deallocate(expanded_topic, allocator.state); + } + + ret = rcutils_string_map_fini(&subs); + ASSERT_EQ(RCL_RET_OK, ret); +} + +TEST(test_expand_topic_name, custom_substitution) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcutils_string_map_t subs = rcutils_get_zero_initialized_string_map(); + rcutils_ret_t rcu_ret = rcutils_string_map_init(&subs, 0, allocator); + ASSERT_EQ(RCUTILS_RET_OK, rcu_ret); + ret = rcl_get_default_topic_name_substitutions(&subs); + ASSERT_EQ(RCL_RET_OK, ret); + + rcu_ret = rcutils_string_map_set(&subs, "ping", "pong"); + ASSERT_EQ(RCUTILS_RET_OK, rcu_ret); + + { + const char * topic = "{ping}"; + const char * ns = "/my_ns"; + const char * node = "my_node"; + char * expanded_topic; + ret = rcl_expand_topic_name(topic, node, ns, &subs, allocator, &expanded_topic); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/my_ns/pong", expanded_topic); + allocator.deallocate(expanded_topic, allocator.state); + } + + ret = rcutils_string_map_fini(&subs); + ASSERT_EQ(RCL_RET_OK, ret); +} diff --git a/test/rcl/test_get_actual_qos.cpp b/test/rcl/test_get_actual_qos.cpp new file mode 100644 index 0000000..f590867 --- /dev/null +++ b/test/rcl/test_get_actual_qos.cpp @@ -0,0 +1,482 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl/publisher.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcutils/logging_macros.h" +#include "rcutils/macros.h" + +#include "test_msgs/msg/basic_types.h" +#include "test_msgs/srv/basic_types.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +# define RMW_IMPLEMENTATION_STR RCUTILS_STRINGIFY(RMW_IMPLEMENTATION) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +#define EXPAND(x) x +#define TEST_FIXTURE_P_RMW(test_fixture_name) CLASSNAME( \ + test_fixture_name, RMW_IMPLEMENTATION) +#define APPLY(macro, ...) EXPAND(macro(__VA_ARGS__)) +#define TEST_P_RMW(test_case_name, test_name) \ + APPLY( \ + TEST_P, \ + CLASSNAME(test_case_name, RMW_IMPLEMENTATION), test_name) +#define INSTANTIATE_TEST_SUITE_P_RMW(instance_name, test_case_name, ...) \ + EXPAND( \ + APPLY( \ + INSTANTIATE_TEST_SUITE_P, instance_name, \ + CLASSNAME(test_case_name, RMW_IMPLEMENTATION), __VA_ARGS__)) + +/** + * Parameterized test. + * The first param are the NodeOptions used to create the nodes. + * The second param are the expect intraprocess count results. + */ +struct TestParameters +{ + rmw_qos_profile_t qos_to_set; + rmw_qos_profile_t qos_expected; + std::string description; +}; + +std::ostream & operator<<( + std::ostream & out, + const TestParameters & params) +{ + out << params.description; + return out; +} + +bool operator==( + const rmw_time_t & lhs, + const rmw_time_t & rhs) +{ + return lhs.sec == rhs.sec && lhs.nsec == rhs.nsec; +} + +bool operator>=( + const rmw_time_t & lhs, + const rmw_time_t & rhs) +{ + if (lhs.sec > rhs.sec) { + return true; + } else if (lhs.sec == rhs.sec) { + return lhs.nsec >= rhs.nsec; + } else { + return false; + } +} + +std::ostream & operator<<( + std::ostream & out, + const rmw_time_t & param) +{ + out << "sec: " << param.sec << " nsec: " << param.nsec; + return out; +} + +class TEST_FIXTURE_P_RMW (TestGetActualQoS) + : public ::testing::TestWithParam +{ +public: + void SetUp() override + { + rcl_ret_t ret; + rcl_node_options_t node_options = rcl_node_get_default_options(); + + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "test_get_actual_qos_node"; + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() override + { + rcl_ret_t ret; + + ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + +protected: + rcl_node_t * node_ptr; + rcl_context_t * context_ptr; +}; + + +class TEST_FIXTURE_P_RMW (TestPublisherGetActualQoS) + : public TEST_FIXTURE_P_RMW(TestGetActualQoS) {}; + +TEST_P_RMW(TestPublisherGetActualQoS, test_publisher_get_qos_settings) +{ + TestParameters parameters = GetParam(); + std::string topic_name("/test_publisher_get_actual_qos__"); + rcl_ret_t ret; + + rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); + pub_ops.qos = parameters.qos_to_set; + auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + ret = rcl_publisher_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + const rmw_qos_profile_t * qos; + qos = rcl_publisher_get_actual_qos(&pub); + EXPECT_NE(nullptr, qos) << rcl_get_error_string().str; + + EXPECT_EQ( + qos->history, + parameters.qos_expected.history); + if (parameters.qos_expected.history == RMW_QOS_POLICY_HISTORY_KEEP_LAST) { + EXPECT_EQ(qos->depth, parameters.qos_expected.depth); + } + EXPECT_EQ( + qos->reliability, + parameters.qos_expected.reliability); + EXPECT_EQ( + qos->durability, + parameters.qos_expected.durability); + EXPECT_GE( + qos->deadline, + parameters.qos_expected.deadline); + EXPECT_GE( + qos->lifespan, + parameters.qos_expected.lifespan); + EXPECT_EQ( + qos->liveliness, + parameters.qos_expected.liveliness); + EXPECT_GE( + qos->liveliness_lease_duration, + parameters.qos_expected.liveliness_lease_duration); + EXPECT_EQ( + qos->avoid_ros_namespace_conventions, + parameters.qos_expected.avoid_ros_namespace_conventions); + + ret = rcl_publisher_fini(&pub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + + +class TEST_FIXTURE_P_RMW (TestSubscriptionGetActualQoS) + : public TEST_FIXTURE_P_RMW(TestGetActualQoS) {}; + +TEST_P_RMW(TestSubscriptionGetActualQoS, test_subscription_get_qos_settings) +{ + TestParameters parameters = GetParam(); + std::string topic_name("/test_subscription_get_qos_settings"); + rcl_ret_t ret; + + rcl_subscription_t pub = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t pub_ops = rcl_subscription_get_default_options(); + pub_ops.qos = parameters.qos_to_set; + auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + ret = rcl_subscription_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + const rmw_qos_profile_t * qos; + qos = rcl_subscription_get_actual_qos(&pub); + EXPECT_NE(nullptr, qos) << rcl_get_error_string().str; + + EXPECT_EQ( + qos->history, + parameters.qos_expected.history); + if (parameters.qos_expected.history == RMW_QOS_POLICY_HISTORY_KEEP_LAST) { + EXPECT_EQ(qos->depth, parameters.qos_expected.depth); + } + EXPECT_EQ( + qos->reliability, + parameters.qos_expected.reliability); + EXPECT_EQ( + qos->durability, + parameters.qos_expected.durability); + EXPECT_GE( + qos->deadline, + parameters.qos_expected.deadline); + // note: lifespan is not a concept in subscriptions + EXPECT_EQ( + qos->liveliness, + parameters.qos_expected.liveliness); + EXPECT_GE( + qos->liveliness_lease_duration, + parameters.qos_expected.liveliness_lease_duration); + EXPECT_EQ( + qos->avoid_ros_namespace_conventions, + parameters.qos_expected.avoid_ros_namespace_conventions); + + ret = rcl_subscription_fini(&pub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + + +// +// other input profile settings +// + +static constexpr rmw_qos_profile_t +nondefault_qos_profile() +{ + rmw_qos_profile_t profile = rmw_qos_profile_default; + profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + profile.depth = 1000; + profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + profile.deadline.sec = 1; + profile.lifespan.nsec = 500000; + profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + profile.liveliness_lease_duration.sec = 1; + profile.avoid_ros_namespace_conventions = true; + return profile; +} + +static constexpr rmw_qos_profile_t +nondefault_qos_profile_for_fastrtps() +{ + rmw_qos_profile_t profile = rmw_qos_profile_default; + profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + profile.depth = 1000; + profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + profile.deadline.sec = 1; + profile.lifespan.nsec = 500000; + profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + // profile.liveliness_lease_duration.sec = 1; // fastrtps does not fully support liveliness + profile.avoid_ros_namespace_conventions = true; + return profile; +} + + +// +// expected output profile settings +// + +static constexpr rmw_qos_profile_t +expected_default_qos_profile() +{ + rmw_qos_profile_t profile = rmw_qos_profile_default; + profile.deadline.sec = 2147483647; + profile.lifespan.sec = 2147483647; + profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + profile.liveliness_lease_duration.sec = 2147483647; + return profile; +} + +static constexpr rmw_qos_profile_t +expected_nondefault_qos_profile() +{ + return nondefault_qos_profile(); +} + +static constexpr rmw_qos_profile_t +expected_nondefault_qos_profile_for_fastrtps() +{ + rmw_qos_profile_t profile = rmw_qos_profile_default; + profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + profile.depth = 1000; + profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + profile.deadline.sec = 1; + profile.lifespan.nsec = 500000; + profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + profile.liveliness_lease_duration.sec = 2147483647; + profile.avoid_ros_namespace_conventions = true; + return profile; +} + +static constexpr rmw_qos_profile_t +expected_system_default_publisher_qos_profile() +{ + rmw_qos_profile_t profile = rmw_qos_profile_default; + profile.depth = 1; + profile.deadline.sec = 2147483647; + profile.lifespan.sec = 2147483647; + profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + profile.liveliness_lease_duration.sec = 2147483647; + return profile; +} + +static constexpr rmw_qos_profile_t +expected_system_default_publisher_qos_profile_for_fastrtps() +{ + rmw_qos_profile_t profile = rmw_qos_profile_default; + profile.depth = 1; + profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + profile.liveliness_lease_duration.sec = 2147483647; + return profile; +} + +static constexpr rmw_qos_profile_t +expected_system_default_subscription_qos_profile() +{ + rmw_qos_profile_t profile = rmw_qos_profile_default; + profile.depth = 1; + profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + profile.deadline.sec = 2147483647; + profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + profile.liveliness_lease_duration.sec = 2147483647; + return profile; +} + +static constexpr rmw_qos_profile_t +expected_system_default_subscription_qos_profile_for_fastrtps() +{ + rmw_qos_profile_t profile = rmw_qos_profile_default; + profile.depth = 1; + profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + profile.deadline.sec = 2147483647; + profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + profile.liveliness_lease_duration.sec = 2147483647; + return profile; +} + + +// +// create set of input and expected output profile setting pairs +// +std::vector +get_parameters(bool for_publisher) +{ + std::vector parameters; + + /* + * Testing with default qos settings. + */ + parameters.push_back( + { + rmw_qos_profile_default, + expected_default_qos_profile(), + "default_qos" + }); + +#ifdef RMW_IMPLEMENTATION_STR + std::string rmw_implementation_str = RMW_IMPLEMENTATION_STR; + if (rmw_implementation_str == "rmw_fastrtps_cpp" || + rmw_implementation_str == "rmw_fastrtps_dynamic_cpp") + { + /* + * Test with non-default settings. + */ + parameters.push_back( + { + nondefault_qos_profile_for_fastrtps(), + expected_nondefault_qos_profile_for_fastrtps(), + "nondefault_qos" + }); + + /* + * Test with system default settings. + */ + if (for_publisher) { + parameters.push_back( + { + rmw_qos_profile_system_default, + expected_system_default_publisher_qos_profile_for_fastrtps(), + "system_default_publisher_qos" + }); + } else { + parameters.push_back( + { + rmw_qos_profile_system_default, + expected_system_default_subscription_qos_profile_for_fastrtps(), + "system_default_publisher_qos" + }); + } + } else { + // TODO(asorbini): Remove this block once ros2/rmw_connext is deprecated. + if (rmw_implementation_str == "rmw_connext_cpp" || + rmw_implementation_str == "rmw_connext_dynamic_cpp") + { + /* + * Test with non-default settings. + */ + parameters.push_back( + { + nondefault_qos_profile(), + expected_nondefault_qos_profile(), + "nondefault_qos" + }); + + /* + * Test with system default settings. + */ + if (for_publisher) { + parameters.push_back( + { + rmw_qos_profile_system_default, + expected_system_default_publisher_qos_profile(), + "system_default_publisher_qos" + }); + } else { + parameters.push_back( + { + rmw_qos_profile_system_default, + expected_system_default_subscription_qos_profile(), + "system_default_publisher_qos" + }); + } + } + } +#endif + + return parameters; +} + +INSTANTIATE_TEST_SUITE_P_RMW( + TestPublisherWithDifferentQoSSettings, + TestPublisherGetActualQoS, + ::testing::ValuesIn(get_parameters(true)), + ::testing::PrintToStringParamName()); + +INSTANTIATE_TEST_SUITE_P_RMW( + TestSubscriptionWithDifferentQoSSettings, + TestSubscriptionGetActualQoS, + ::testing::ValuesIn(get_parameters(false)), + ::testing::PrintToStringParamName()); diff --git a/test/rcl/test_get_node_names.cpp b/test/rcl/test_get_node_names.cpp new file mode 100644 index 0000000..80a0da4 --- /dev/null +++ b/test/rcl/test_get_node_names.cpp @@ -0,0 +1,299 @@ +// 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 +#include +#include +#include +#include +#include +#include + +#include "rcutils/types.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/graph.h" +#include "rcl/rcl.h" +#include "rmw/rmw.h" + +#include "rcl/error_handling.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +using namespace std::chrono_literals; + +class CLASSNAME (TestGetNodeNames, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + void SetUp() + {} + + void TearDown() + {} +}; + +TEST_F(CLASSNAME(TestGetNodeNames, RMW_IMPLEMENTATION), test_rcl_get_node_names) { + rcl_ret_t ret; + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_shutdown(&context)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_context_fini(&context)) << rcl_get_error_string().str; + }); + std::multiset> expected_nodes, discovered_nodes; + + auto node1_ptr = new rcl_node_t; + *node1_ptr = rcl_get_zero_initialized_node(); + const char * node1_name = "node1"; + const char * node1_namespace = "/"; + rcl_node_options_t node1_options = rcl_node_get_default_options(); + ret = rcl_node_init(node1_ptr, node1_name, node1_namespace, &context, &node1_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + expected_nodes.insert(std::make_pair(std::string(node1_name), std::string(node1_namespace))); + + auto node2_ptr = new rcl_node_t; + *node2_ptr = rcl_get_zero_initialized_node(); + const char * node2_name = "node2"; + const char * node2_namespace = "/"; + rcl_node_options_t node2_options = rcl_node_get_default_options(); + ret = rcl_node_init(node2_ptr, node2_name, node2_namespace, &context, &node2_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + expected_nodes.insert(std::make_pair(std::string(node2_name), std::string(node2_namespace))); + + auto node3_ptr = new rcl_node_t; + *node3_ptr = rcl_get_zero_initialized_node(); + const char * node3_name = "node3"; + const char * node3_namespace = "/ns"; + rcl_node_options_t node3_options = rcl_node_get_default_options(); + ret = rcl_node_init(node3_ptr, node3_name, node3_namespace, &context, &node3_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + expected_nodes.insert(std::make_pair(std::string(node3_name), std::string(node3_namespace))); + + auto node4_ptr = new rcl_node_t; + *node4_ptr = rcl_get_zero_initialized_node(); + const char * node4_name = "node2"; + const char * node4_namespace = "/ns/ns"; + rcl_node_options_t node4_options = rcl_node_get_default_options(); + ret = rcl_node_init(node4_ptr, node4_name, node4_namespace, &context, &node4_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + expected_nodes.insert(std::make_pair(std::string(node4_name), std::string(node4_namespace))); + + auto node5_ptr = new rcl_node_t; + *node5_ptr = rcl_get_zero_initialized_node(); + const char * node5_name = "node1"; + const char * node5_namespace = "/"; + rcl_node_options_t node5_options = rcl_node_get_default_options(); + ret = rcl_node_init(node5_ptr, node5_name, node5_namespace, &context, &node5_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + expected_nodes.insert(std::make_pair(std::string(node5_name), std::string(node5_namespace))); + + std::this_thread::sleep_for(1s); + + rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); + ret = rcl_get_node_names(node1_ptr, node1_options.allocator, &node_names, &node_namespaces); + ASSERT_EQ(RCUTILS_RET_OK, ret) << rcl_get_error_string().str; + + std::stringstream ss; + ss << "[test_rcl_get_node_names]: Found node names:" << std::endl; + for (size_t i = 0; i < node_names.size; ++i) { + ss << node_names.data[i] << std::endl; + } + EXPECT_EQ(node_names.size, node_namespaces.size) << ss.str(); + + for (size_t i = 0; i < node_names.size; ++i) { + discovered_nodes.insert( + std::make_pair( + std::string(node_names.data[i]), + std::string(node_namespaces.data[i]))); + } + EXPECT_EQ(discovered_nodes, expected_nodes); + + ret = rcutils_string_array_fini(&node_names); + ASSERT_EQ(RCUTILS_RET_OK, ret); + + ret = rcutils_string_array_fini(&node_namespaces); + ASSERT_EQ(RCUTILS_RET_OK, ret); + + ret = rcl_node_fini(node1_ptr); + delete node1_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(node2_ptr); + delete node2_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(node3_ptr); + delete node3_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(node4_ptr); + delete node4_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(node5_ptr); + delete node5_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +TEST_F( + CLASSNAME(TestGetNodeNames, RMW_IMPLEMENTATION), test_rcl_get_node_names_with_enclave) +{ + rcl_ret_t ret; + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + rcl_context_t context = rcl_get_zero_initialized_context(); + const char * enclave_name = "/enclave"; + const char * argv[] = {"--ros-args", "--enclave", enclave_name}; + ret = rcl_init(3, argv, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_shutdown(&context)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_context_fini(&context)) << rcl_get_error_string().str; + }); + std::multiset> + expected_nodes, discovered_nodes; + + rcl_node_t node1 = rcl_get_zero_initialized_node(); + const char * node1_name = "node1"; + const char * node1_namespace = "/"; + rcl_node_options_t node1_options = rcl_node_get_default_options(); + ret = rcl_node_init(&node1, node1_name, node1_namespace, &context, &node1_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + expected_nodes.insert( + std::make_tuple( + std::string(node1_name), + std::string(node1_namespace), + std::string(enclave_name))); + + rcl_node_t node2 = rcl_get_zero_initialized_node(); + const char * node2_name = "node2"; + const char * node2_namespace = "/"; + rcl_node_options_t node2_options = rcl_node_get_default_options(); + ret = rcl_node_init(&node2, node2_name, node2_namespace, &context, &node2_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + expected_nodes.insert( + std::make_tuple( + std::string(node2_name), + std::string(node2_namespace), + std::string(enclave_name))); + + rcl_node_t node3 = rcl_get_zero_initialized_node(); + const char * node3_name = "node3"; + const char * node3_namespace = "/ns"; + rcl_node_options_t node3_options = rcl_node_get_default_options(); + ret = rcl_node_init(&node3, node3_name, node3_namespace, &context, &node3_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + expected_nodes.insert( + std::make_tuple( + std::string(node3_name), + std::string(node3_namespace), + std::string(enclave_name))); + + rcl_node_t node4 = rcl_get_zero_initialized_node(); + const char * node4_name = "node2"; + const char * node4_namespace = "/ns/ns"; + rcl_node_options_t node4_options = rcl_node_get_default_options(); + ret = rcl_node_init(&node4, node4_name, node4_namespace, &context, &node4_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + expected_nodes.insert( + std::make_tuple( + std::string(node4_name), + std::string(node4_namespace), + std::string(enclave_name))); + + rcl_node_t node5 = rcl_get_zero_initialized_node(); + const char * node5_name = "node1"; + const char * node5_namespace = "/"; + rcl_node_options_t node5_options = rcl_node_get_default_options(); + ret = rcl_node_init(&node5, node5_name, node5_namespace, &context, &node5_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + expected_nodes.insert( + std::make_tuple( + std::string(node5_name), + std::string(node5_namespace), + std::string(enclave_name))); + + std::this_thread::sleep_for(1s); + + rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t enclaves = rcutils_get_zero_initialized_string_array(); + ret = rcl_get_node_names_with_enclaves( + &node1, node1_options.allocator, &node_names, &node_namespaces, &enclaves); + ASSERT_EQ(RCUTILS_RET_OK, ret) << rcl_get_error_string().str; + + std::stringstream ss; + ss << "[test_rcl_get_node_names]: Found node names:" << std::endl; + for (size_t i = 0; i < node_names.size; ++i) { + ss << node_names.data[i] << std::endl; + } + EXPECT_EQ(node_names.size, node_namespaces.size) << ss.str(); + + for (size_t i = 0; i < node_names.size; ++i) { + discovered_nodes.insert( + std::make_tuple( + std::string(node_names.data[i]), + std::string(node_namespaces.data[i]), + std::string(enclaves.data[i]))); + } + EXPECT_EQ(discovered_nodes, expected_nodes); + + ret = rcutils_string_array_fini(&node_names); + ASSERT_EQ(RCUTILS_RET_OK, ret); + + ret = rcutils_string_array_fini(&node_namespaces); + ASSERT_EQ(RCUTILS_RET_OK, ret); + + ret = rcutils_string_array_fini(&enclaves); + ASSERT_EQ(RCUTILS_RET_OK, ret); + + ret = rcl_node_fini(&node1); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(&node2); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(&node3); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(&node4); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(&node5); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} diff --git a/test/rcl/test_graph.cpp b/test/rcl/test_graph.cpp new file mode 100644 index 0000000..3410325 --- /dev/null +++ b/test/rcl/test_graph.cpp @@ -0,0 +1,1701 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Disable -Wmissing-field-initializers, as it is overly strict and will be +// relaxed in future versions of GCC (it is not a warning for clang). +// See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=36750#c13 +#ifndef _WIN32 +# pragma GCC diagnostic ignored "-Wmissing-field-initializers" +#endif + +#include + +#include +#include +#include +#include +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/graph.h" +#include "rcl/logging.h" +#include "rcl/rcl.h" + +#include "rcutils/logging_macros.h" +#include "rcutils/logging.h" + +#include "test_msgs/msg/basic_types.h" +#include "test_msgs/srv/basic_types.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +bool is_connext = + std::string(rmw_get_implementation_identifier()).find("rmw_connext") == 0; + +class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t * old_context_ptr; + rcl_context_t * context_ptr; + rcl_node_t * old_node_ptr; + rcl_node_t * node_ptr; + rcl_wait_set_t * wait_set_ptr; + const char * test_graph_node_name = "test_graph_node"; + + void SetUp() + { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->old_context_ptr = new rcl_context_t; + *this->old_context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->old_context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_OK, + rcl_logging_configure(&this->old_context_ptr->global_arguments, &allocator) + ) << rcl_get_error_string().str; + this->old_node_ptr = new rcl_node_t; + *this->old_node_ptr = rcl_get_zero_initialized_node(); + const char * old_name = "old_node_name"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->old_node_ptr, old_name, "", this->old_context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->old_context_ptr); // after this, the old_node_ptr should be invalid + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "test_graph_node"; + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->wait_set_ptr = new rcl_wait_set_t; + *this->wait_set_ptr = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init( + this->wait_set_ptr, 0, 1, 0, 0, 0, 0, this->context_ptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret; + ret = rcl_node_fini(this->old_node_ptr); + delete this->old_node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_wait_set_fini(this->wait_set_ptr); + delete this->wait_set_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + delete this->context_ptr; + ret = rcl_context_fini(this->old_context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + delete this->old_context_ptr; + EXPECT_EQ(RCL_RET_OK, rcl_logging_fini()) << rcl_get_error_string().str; + } +}; + +/* Test the rcl_get_topic_names_and_types and rcl_destroy_topic_names_and_types functions. + * + * This does not test content of the rcl_names_and_types_t structure. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_get_and_destroy_topic_names_and_types +) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); + rcl_names_and_types_t tnat = rcl_get_zero_initialized_names_and_types(); + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + // invalid node + ret = rcl_get_topic_names_and_types(nullptr, &allocator, false, &tnat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_topic_names_and_types(&zero_node, &allocator, false, &tnat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_topic_names_and_types(this->old_node_ptr, &allocator, false, &tnat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid allocator + ret = rcl_get_topic_names_and_types(this->node_ptr, nullptr, false, &tnat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_topic_names_and_types(this->node_ptr, &zero_allocator, false, &tnat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid topic_names_and_types + ret = rcl_get_topic_names_and_types(this->node_ptr, &allocator, false, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + tnat.names.size = 1; + ret = rcl_get_topic_names_and_types(this->node_ptr, &allocator, false, &tnat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + tnat.names.size = 0; + // invalid argument to rcl_destroy_topic_names_and_types + ret = rcl_names_and_types_fini(nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // valid calls + ret = rcl_get_topic_names_and_types(this->node_ptr, &allocator, false, &tnat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_names_and_types_fini(&tnat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +/* Test the rcl_get_service_names_and_types function. + * + * This does not test content of the rcl_names_and_types_t structure. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_get_service_names_and_types +) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); + rcl_names_and_types_t tnat = rcl_get_zero_initialized_names_and_types(); + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + // invalid node + ret = rcl_get_service_names_and_types(nullptr, &allocator, &tnat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types(&zero_node, &allocator, &tnat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types(this->old_node_ptr, &allocator, &tnat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid allocator + ret = rcl_get_service_names_and_types(this->node_ptr, nullptr, &tnat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types(this->node_ptr, &zero_allocator, &tnat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid service_names_and_types + ret = rcl_get_service_names_and_types(this->node_ptr, &allocator, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + tnat.names.size = 1; + ret = rcl_get_service_names_and_types(this->node_ptr, &allocator, &tnat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + tnat.names.size = 0; + // invalid argument to rcl_destroy_service_names_and_types + ret = rcl_names_and_types_fini(nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // valid calls + ret = rcl_get_service_names_and_types(this->node_ptr, &allocator, &tnat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_names_and_types_fini(&tnat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +/* Test the rcl_names_and_types_init function. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_names_and_types_init +) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + // invalid names and types + ret = rcl_names_and_types_init(nullptr, 10, &allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid allocator + ret = rcl_names_and_types_init(&nat, 10, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_names_and_types_init(&nat, 10, &zero_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // zero size + ret = rcl_names_and_types_init(&nat, 0, &allocator); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(nat.names.size, 0u); + ret = rcl_names_and_types_fini(&nat); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // non-zero size + size_t num_names = 10u; + ret = rcl_names_and_types_init(&nat, num_names, &allocator); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(nat.names.size, num_names); + for (size_t i = 0; i < num_names; i++) { + EXPECT_EQ(nat.types[i].size, 0u); + } + ret = rcl_names_and_types_fini(&nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +/* Test the rcl_get_publisher_names_and_types_by_node function. + * + * This does not test content of the response. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_get_publisher_names_and_types_by_node +) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + const char * unknown_node_name = "test_rcl_get_publisher_names_and_types_by_node"; + const char * unknown_node_ns = "/test/namespace"; + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + // invalid node + ret = rcl_get_publisher_names_and_types_by_node( + nullptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_publisher_names_and_types_by_node( + &zero_node, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_publisher_names_and_types_by_node( + this->old_node_ptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid allocator + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, nullptr, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &zero_allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid names + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, nullptr, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, nullptr, &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // test valid strings with invalid node names + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, "", "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAME, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, "_!InvalidNodeName", "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAME, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "_!invalidNs", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAMESPACE, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid names and types + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "", nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + nat.names.size = 1; + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + nat.names.size = 0; + // unknown node name + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, unknown_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_NAME_NON_EXISTENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // unknown node namespace + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, unknown_node_ns, &nat); + EXPECT_EQ(RCL_RET_NODE_NAME_NON_EXISTENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // valid call + ret = rcl_get_publisher_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_names_and_types_fini(&nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +/* Test the rcl_get_subscriber_names_and_types_by_node function. + * + * This does not test content of the response. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriber_names_and_types_by_node +) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + const char * unknown_node_name = "test_rcl_get_subscriber_names_and_types_by_node"; + const char * unknown_node_ns = "/test/namespace"; + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + // invalid node + ret = rcl_get_subscriber_names_and_types_by_node( + nullptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_subscriber_names_and_types_by_node( + &zero_node, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_subscriber_names_and_types_by_node( + this->old_node_ptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid allocator + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, nullptr, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &zero_allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid names + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, nullptr, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, nullptr, &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // test valid strings with invalid node names + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, "", "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAME, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, "_!InvalidNodeName", "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAME, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "_!invalidNs", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAMESPACE, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid names and types + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "", nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + nat.names.size = 1; + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + nat.names.size = 0; + // unknown node name + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, unknown_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_NAME_NON_EXISTENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // unknown node namespace + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, unknown_node_ns, &nat); + EXPECT_EQ(RCL_RET_NODE_NAME_NON_EXISTENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // valid call + ret = rcl_get_subscriber_names_and_types_by_node( + this->node_ptr, &allocator, false, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +/* Test the rcl_get_service_names_and_types_by_node function. + * + * This does not test content of the response. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_get_service_names_and_types_by_node +) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + const char * unknown_node_name = "test_rcl_get_service_names_and_types_by_node"; + const char * unknown_node_ns = "/test/namespace"; + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + // invalid node + ret = rcl_get_service_names_and_types_by_node( + nullptr, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types_by_node( + &zero_node, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types_by_node( + this->old_node_ptr, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid allocator + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, nullptr, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &zero_allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid names + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, nullptr, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, nullptr, &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // test valid strings with invalid node names + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, "", "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAME, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, "_!InvalidNodeName", "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAME, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, "_!invalidNs", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAMESPACE, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid names and types + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, "", nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + nat.names.size = 1; + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + nat.names.size = 0; + // unknown node name + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, unknown_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_NAME_NON_EXISTENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // unknown node namespace + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, unknown_node_ns, &nat); + EXPECT_EQ(RCL_RET_NODE_NAME_NON_EXISTENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // valid call + ret = rcl_get_service_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +/* Test the rcl_get_client_names_and_types_by_node function. + * + * This does not test content of the response. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_get_client_names_and_types_by_node +) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + const char * unknown_node_name = "test_rcl_get_client_names_and_types_by_node"; + const char * unknown_node_ns = "/test/namespace"; + + rcl_names_and_types_t nat = rcl_get_zero_initialized_names_and_types(); + // invalid node + ret = rcl_get_client_names_and_types_by_node( + nullptr, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_client_names_and_types_by_node( + &zero_node, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_client_names_and_types_by_node( + this->old_node_ptr, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid allocator + ret = rcl_get_client_names_and_types_by_node( + this->node_ptr, nullptr, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_client_names_and_types_by_node( + this->node_ptr, &zero_allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid names + ret = rcl_get_client_names_and_types_by_node( + this->node_ptr, &allocator, nullptr, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_client_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, nullptr, &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // test valid strings with invalid node names + ret = rcl_get_client_names_and_types_by_node( + this->node_ptr, &allocator, "", "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAME, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_client_names_and_types_by_node( + this->node_ptr, &allocator, "_!InvalidNodeName", "", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAME, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_get_client_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, "_!invalidNs", &nat); + EXPECT_EQ(RCL_RET_NODE_INVALID_NAMESPACE, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid names and types + ret = rcl_get_client_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, "", nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + nat.names.size = 1; + ret = rcl_get_client_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + nat.names.size = 0; + // unknown node name + ret = rcl_get_client_names_and_types_by_node( + this->node_ptr, &allocator, unknown_node_name, "", &nat); + EXPECT_EQ(RCL_RET_NODE_NAME_NON_EXISTENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // unknown node namespace + ret = rcl_get_client_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, unknown_node_ns, &nat); + EXPECT_EQ(RCL_RET_NODE_NAME_NON_EXISTENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // valid call + ret = rcl_get_client_names_and_types_by_node( + this->node_ptr, &allocator, this->test_graph_node_name, "", &nat); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +/* Test the rcl_count_publishers function. + * + * This does not test content of the response. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_count_publishers +) { + rcl_ret_t ret; + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + const char * topic_name = "/topic_test_rcl_count_publishers"; + size_t count; + // invalid node + ret = rcl_count_publishers(nullptr, topic_name, &count); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_count_publishers(&zero_node, topic_name, &count); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_count_publishers(this->old_node_ptr, topic_name, &count); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid topic name + ret = rcl_count_publishers(this->node_ptr, nullptr, &count); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // TODO(wjwwood): test valid strings with invalid topic names in them + // invalid count + ret = rcl_count_publishers(this->node_ptr, topic_name, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // valid call + ret = rcl_count_publishers(this->node_ptr, topic_name, &count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +/* Test the rcl_count_subscribers function. + * + * This does not test content of the response. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_count_subscribers +) { + rcl_ret_t ret; + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + const char * topic_name = "/topic_test_rcl_count_subscribers"; + size_t count; + // invalid node + ret = rcl_count_subscribers(nullptr, topic_name, &count); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_count_subscribers(&zero_node, topic_name, &count); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_count_subscribers(this->old_node_ptr, topic_name, &count); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // invalid topic name + ret = rcl_count_subscribers(this->node_ptr, nullptr, &count); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // TODO(wjwwood): test valid strings with invalid topic names in them + // invalid count + ret = rcl_count_subscribers(this->node_ptr, topic_name, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // valid call + ret = rcl_count_subscribers(this->node_ptr, topic_name, &count); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +/* Test the rcl_wait_for_publishers function. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_wait_for_publishers +) { + rcl_ret_t ret; + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); + rcl_allocator_t allocator = rcl_get_default_allocator(); + const char * topic_name = "/topic_test_rcl_wait_for_publishers"; + bool success = false; + + // Invalid node + ret = rcl_wait_for_publishers(nullptr, &allocator, topic_name, 1u, 100, &success); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + ret = rcl_wait_for_publishers(&zero_node, &allocator, topic_name, 1u, 100, &success); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + ret = rcl_wait_for_publishers(this->old_node_ptr, &allocator, topic_name, 1u, 100, &success); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid allocator + ret = rcl_wait_for_publishers(this->node_ptr, nullptr, topic_name, 1u, 100, &success); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_wait_for_publishers(this->node_ptr, &zero_allocator, topic_name, 1u, 100, &success); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid topic name + ret = rcl_wait_for_publishers(this->node_ptr, &allocator, nullptr, 1u, 100, &success); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid output arg + ret = rcl_wait_for_publishers(this->node_ptr, &allocator, topic_name, 1u, 100, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Valid call (expect timeout since there are no publishers) + ret = rcl_wait_for_publishers(this->node_ptr, &allocator, topic_name, 1u, 100, &success); + EXPECT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +/* Test the rcl_wait_for_subscribers function. + */ +TEST_F( + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), + test_rcl_wait_for_subscribers +) { + rcl_ret_t ret; + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + rcl_allocator_t zero_allocator = static_cast( + rcutils_get_zero_initialized_allocator()); + rcl_allocator_t allocator = rcl_get_default_allocator(); + const char * topic_name = "/topic_test_rcl_wait_for_subscribers"; + bool success = false; + + // Invalid node + ret = rcl_wait_for_subscribers(nullptr, &allocator, topic_name, 1u, 100, &success); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + ret = rcl_wait_for_subscribers(&zero_node, &allocator, topic_name, 1u, 100, &success); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + ret = rcl_wait_for_subscribers(this->old_node_ptr, &allocator, topic_name, 1u, 100, &success); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid allocator + ret = rcl_wait_for_subscribers(this->node_ptr, nullptr, topic_name, 1u, 100, &success); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_wait_for_subscribers(this->node_ptr, &zero_allocator, topic_name, 1u, 100, &success); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid topic name + ret = rcl_wait_for_subscribers(this->node_ptr, &allocator, nullptr, 1u, 100, &success); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Invalid output arg + ret = rcl_wait_for_subscribers(this->node_ptr, &allocator, topic_name, 1u, 100, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Valid call (expect timeout since there are no subscribers) + ret = rcl_wait_for_subscribers(this->node_ptr, &allocator, topic_name, 1u, 100, &success); + EXPECT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +void +check_entity_count( + const rcl_node_t * node_ptr, + std::string & topic_name, + size_t expected_publisher_count, + size_t expected_subscriber_count, + bool expected_in_tnat, + std::chrono::seconds timeout) +{ + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, + "Expecting number of %zu publishers, %zu subscribers, and that the topic is%s in the graph.", + expected_publisher_count, + expected_subscriber_count, + expected_in_tnat ? "" : " not" + ); + bool is_in_tnat = false; + rcl_names_and_types_t tnat {}; + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + size_t pub_count, sub_count; + + // Check number of entities until timeout expires. + auto start_time = std::chrono::system_clock::now(); + do { + ret = rcl_count_publishers(node_ptr, topic_name.c_str(), &pub_count); + ASSERT_EQ(ret, RCL_RET_OK); + ret = rcl_count_subscribers(node_ptr, topic_name.c_str(), &sub_count); + ASSERT_EQ(ret, RCL_RET_OK); + if ((expected_publisher_count == pub_count) && + (expected_subscriber_count == sub_count)) + { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } while (std::chrono::system_clock::now() - start_time < timeout); + EXPECT_EQ(expected_publisher_count, pub_count); + EXPECT_EQ(expected_subscriber_count, sub_count); + + tnat = rcl_get_zero_initialized_names_and_types(); + ret = rcl_get_topic_names_and_types(node_ptr, &allocator, false, &tnat); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + is_in_tnat = false; + for (size_t i = 0; i < tnat.names.size; ++i) { + if (topic_name == std::string(tnat.names.data[i])) { + ASSERT_FALSE(is_in_tnat) << "duplicates in the tnat"; // Found it more than once! + is_in_tnat = true; + } + } + if (RCL_RET_OK == ret) { + ret = rcl_names_and_types_fini(&tnat); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + if (expected_in_tnat) { + EXPECT_TRUE(is_in_tnat); + } else { + EXPECT_FALSE(is_in_tnat); + } +} + +/** + * Type define a get topics function. + */ +typedef std::function< + rcl_ret_t(const rcl_node_t *, const char * node_name, rcl_names_and_types_t *) +> GetTopicsFunc; + +/** + * Expect a certain number of topics on a given subsystem. + */ +void expect_topics_types( + const rcl_node_t * node, + const GetTopicsFunc & func, + size_t num_topics, + const char * topic_name, + bool expect, + bool & is_success) +{ + rcl_ret_t ret; + rcl_names_and_types_t nat{}; + nat = rcl_get_zero_initialized_names_and_types(); + ret = func(node, topic_name, &nat); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + is_success &= num_topics == nat.names.size; + if (expect) { + EXPECT_EQ(num_topics, nat.names.size); + } else { + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, "Expected topics %zu, actual topics %zu", num_topics, + nat.names.size); + } + ret = rcl_names_and_types_fini(&nat); + ASSERT_EQ(RCL_RET_OK, ret); + rcl_reset_error(); +} + +/** + * Expected state of a node. + */ +struct expected_node_state +{ + size_t publishers; + size_t subscribers; + size_t services; + size_t clients; +}; + +/** + * Extend the TestGraphFixture with a multi node fixture for node discovery and node-graph perspective. + */ +class NodeGraphMultiNodeFixture : public CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION) +{ +public: + const char * remote_node_name = "remote_graph_node"; + std::string topic_name = "/test_node_info_functions__"; + rcl_node_t * remote_node_ptr; + rcl_allocator_t allocator = rcl_get_default_allocator(); + GetTopicsFunc sub_func, pub_func, service_func, client_func; + rcl_context_t * remote_context_ptr; + + void SetUp() override + { + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION) ::SetUp(); + rcl_ret_t ret; + + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << + rcl_get_error_string().str; + }); + + remote_node_ptr = new rcl_node_t; + *remote_node_ptr = rcl_get_zero_initialized_node(); + rcl_node_options_t node_options = rcl_node_get_default_options(); + + this->remote_context_ptr = new rcl_context_t; + *this->remote_context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->remote_context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_init( + remote_node_ptr, remote_node_name, "", this->remote_context_ptr, + &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + sub_func = std::bind( + rcl_get_subscriber_names_and_types_by_node, + std::placeholders::_1, + &this->allocator, + false, + std::placeholders::_2, + "/", + std::placeholders::_3); + pub_func = std::bind( + rcl_get_publisher_names_and_types_by_node, + std::placeholders::_1, + &this->allocator, + false, + std::placeholders::_2, + "/", + std::placeholders::_3); + service_func = std::bind( + rcl_get_service_names_and_types_by_node, + std::placeholders::_1, + &this->allocator, + std::placeholders::_2, + "/", + std::placeholders::_3); + client_func = std::bind( + rcl_get_client_names_and_types_by_node, + std::placeholders::_1, + &this->allocator, + std::placeholders::_2, + "/", + std::placeholders::_3); + wait_for_all_nodes_alive(); + } + + void TearDown() override + { + rcl_ret_t ret; + CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION) ::TearDown(); + ret = rcl_node_fini(this->remote_node_ptr); + + delete this->remote_node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Tearing down class"); + + ret = rcl_shutdown(this->remote_context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->remote_context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + delete this->remote_context_ptr; + } + + void wait_for_all_nodes_alive() + { + // wait for all 3 nodes to be discovered: remote_node, old_node, node + size_t attempts = 0u; + size_t max_attempts = 10u; + size_t last_size = 0u; + do { + std::this_thread::sleep_for(std::chrono::seconds(1)); + rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); + ASSERT_EQ( + RCL_RET_OK, + rcl_get_node_names(this->remote_node_ptr, allocator, &node_names, &node_namespaces)); + attempts++; + last_size = node_names.size; + ASSERT_EQ(RCUTILS_RET_OK, rcutils_string_array_fini(&node_names)); + ASSERT_EQ(RCUTILS_RET_OK, rcutils_string_array_fini(&node_namespaces)); + ASSERT_LE(attempts, max_attempts) << "Unable to attain all required nodes"; + } while (last_size < 3u); + } + + /** + * Verify the number of subsystems each node should have. + * + * \param node_state expected state of node + * \param remote_node_state expected state of remote node + */ + void VerifySubsystemCount( + const expected_node_state && node_state, + const expected_node_state && remote_node_state) const + { + std::vector node_vec; + node_vec.push_back(this->node_ptr); + node_vec.push_back(this->remote_node_ptr); + + size_t attempts = 20; + bool is_expect = false; + rcl_ret_t ret; + + for (size_t i = 0; i < attempts; ++i) { + if (attempts - 1 == i) {is_expect = true;} + bool is_success = true; + // verify each node contains the same node graph. + for (auto node : node_vec) { + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking subscribers from node"); + expect_topics_types( + node, sub_func, node_state.subscribers, + test_graph_node_name, is_expect, is_success); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking services from node"); + expect_topics_types( + node, service_func, node_state.services, + test_graph_node_name, is_expect, is_success); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking clients from node"); + expect_topics_types( + node, client_func, node_state.clients, + test_graph_node_name, is_expect, is_success); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking publishers from node"); + expect_topics_types( + node, pub_func, node_state.publishers, + test_graph_node_name, is_expect, is_success); + + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking subscribers from remote node"); + expect_topics_types( + node, sub_func, remote_node_state.subscribers, + this->remote_node_name, is_expect, is_success); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking publishers from remote node"); + expect_topics_types( + node, pub_func, remote_node_state.publishers, + this->remote_node_name, is_expect, is_success); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking services from remote node"); + expect_topics_types( + node, service_func, remote_node_state.services, + this->remote_node_name, is_expect, is_success); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Checking clients from remote node"); + expect_topics_types( + node, client_func, remote_node_state.clients, + this->remote_node_name, is_expect, is_success); + if (!is_success) { + ret = rcl_wait_set_clear(wait_set_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = + rcl_wait_set_add_guard_condition( + wait_set_ptr, rcl_node_get_graph_guard_condition( + node), NULL); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(400); + RCUTILS_LOG_DEBUG_NAMED( + ROS_PACKAGE_NAME, + " state wrong, waiting up to '%s' nanoseconds for graph changes... ", + std::to_string(time_to_sleep.count()).c_str()); + ret = rcl_wait(wait_set_ptr, time_to_sleep.count()); + if (ret == RCL_RET_TIMEOUT) { + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "timeout"); + continue; + } + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "change occurred"); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + break; + } + } + if (is_success) { + break; + } + } + } +}; + +TEST_F(NodeGraphMultiNodeFixture, test_node_info_subscriptions) +{ + rcl_ret_t ret; + auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + // Create two subscribers. + rcl_subscription_t sub = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&sub, this->node_ptr, ts, this->topic_name.c_str(), &sub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + rcl_subscription_t sub2 = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t sub_ops2 = rcl_subscription_get_default_options(); + ret = + rcl_subscription_init(&sub2, this->remote_node_ptr, ts, this->topic_name.c_str(), &sub_ops2); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + VerifySubsystemCount(expected_node_state{1, 1, 0, 0}, expected_node_state{1, 1, 0, 0}); + + // Destroy the node's subscriber + ret = rcl_subscription_fini(&sub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + VerifySubsystemCount(expected_node_state{1, 0, 0, 0}, expected_node_state{1, 1, 0, 0}); + + // Destroy the remote node's subdscriber + ret = rcl_subscription_fini(&sub2, this->remote_node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + VerifySubsystemCount(expected_node_state{1, 0, 0, 0}, expected_node_state{1, 0, 0, 0}); +} + +TEST_F(NodeGraphMultiNodeFixture, test_node_info_publishers) +{ + rcl_ret_t ret; + // Now create a publisher on "topic_name" and check that it is seen. + rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); + auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + ret = rcl_publisher_init(&pub, this->node_ptr, ts, this->topic_name.c_str(), &pub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + VerifySubsystemCount(expected_node_state{2, 0, 0, 0}, expected_node_state{1, 0, 0, 0}); + + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Destroyed publisher"); + // Destroy the publisher. + ret = rcl_publisher_fini(&pub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + VerifySubsystemCount(expected_node_state{1, 0, 0, 0}, expected_node_state{1, 0, 0, 0}); +} + +TEST_F(NodeGraphMultiNodeFixture, test_node_info_services) +{ + rcl_ret_t ret; + const char * service_name = "test_service"; + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + auto ts1 = ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + ret = rcl_service_init(&service, this->node_ptr, ts1, service_name, &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + VerifySubsystemCount(expected_node_state{1, 0, 1, 0}, expected_node_state{1, 0, 0, 0}); + + // Destroy service. + ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + VerifySubsystemCount(expected_node_state{1, 0, 0, 0}, expected_node_state{1, 0, 0, 0}); +} + +TEST_F(NodeGraphMultiNodeFixture, test_node_info_clients) +{ + rcl_ret_t ret; + const char * service_name = "test_service"; + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options = rcl_client_get_default_options(); + auto ts = ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + ret = rcl_client_init(&client, this->node_ptr, ts, service_name, &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + VerifySubsystemCount(expected_node_state{1, 0, 0, 1}, expected_node_state{1, 0, 0, 0}); + + // Destroy client + ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + VerifySubsystemCount(expected_node_state{1, 0, 0, 0}, expected_node_state{1, 0, 0, 0}); +} + +/* + * Test graph queries with a hand crafted graph. + */ +TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functions) +{ + std::string topic_name("/test_graph_query_functions__"); + std::chrono::nanoseconds now = std::chrono::system_clock::now().time_since_epoch(); + topic_name += std::to_string(now.count()); + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, "Using topic name: %s", topic_name.c_str()); + rcl_ret_t ret; + // First assert the "topic_name" is not in use. + check_entity_count( + this->node_ptr, + topic_name, + 0, // expected publishers on topic + 0, // expected subscribers on topic + false, // topic expected in graph + std::chrono::seconds(4)); // timeout + // Now create a publisher on "topic_name" and check that it is seen. + rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); + auto ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + ret = rcl_publisher_init(&pub, this->node_ptr, ts, topic_name.c_str(), &pub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Check the graph. + check_entity_count( + this->node_ptr, + topic_name, + 1, // expected publishers on topic + 0, // expected subscribers on topic + true, // topic expected in graph + std::chrono::seconds(4)); // timeout + // Now create a subscriber. + rcl_subscription_t sub = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&sub, this->node_ptr, ts, topic_name.c_str(), &sub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Check the graph again. + check_entity_count( + this->node_ptr, + topic_name, + 1, // expected publishers on topic + 1, // expected subscribers on topic + true, // topic expected in graph + std::chrono::seconds(4)); // timeout + // Destroy the publisher. + ret = rcl_publisher_fini(&pub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Check the graph again. + check_entity_count( + this->node_ptr, + topic_name, + 0, // expected publishers on topic + 1, // expected subscribers on topic + true, // topic expected in graph + std::chrono::seconds(4)); // timeout + // Destroy the subscriber. + ret = rcl_subscription_fini(&sub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + // Check the graph again. + check_entity_count( + this->node_ptr, + topic_name, + 0, // expected publishers on topic + 0, // expected subscribers on topic + false, // topic expected in graph + std::chrono::seconds(4)); // timeout +} + +/* Test the graph guard condition notices below changes. + * publisher create/destroy, subscription create/destroy + * service create/destroy, client create/destroy + * Other node added/removed + * + * Note: this test could be impacted by other communications on the same ROS Domain. + */ +TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_condition_trigger_check) { +#define CHECK_GUARD_CONDITION_CHANGE(EXPECTED_RESULT, TIMEOUT) do { \ + ret = rcl_wait_set_clear(&wait_set); \ + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + ret = rcl_wait_set_add_guard_condition(&wait_set, graph_guard_condition, NULL); \ + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + ret = rcl_wait(&wait_set, TIMEOUT.count()); \ + ASSERT_EQ(EXPECTED_RESULT, ret) << rcl_get_error_string().str; \ +} while (0) + + rcl_ret_t ret; + std::chrono::nanoseconds timeout_1s = std::chrono::seconds(1); + std::chrono::nanoseconds timeout_3s = std::chrono::seconds(3); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init( + &wait_set, 0, 1, 0, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + }); + + const rcl_guard_condition_t * graph_guard_condition = + rcl_node_get_graph_guard_condition(node_ptr); + + // Wait for no graph change condition + int idx = 0; + for (; idx < 100; idx++) { + ret = rcl_wait_set_clear(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_guard_condition(&wait_set, graph_guard_condition, NULL); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait(&wait_set, timeout_3s.count()); + if (RCL_RET_TIMEOUT == ret) { + break; + } else { + RCUTILS_LOG_INFO_NAMED( + ROS_PACKAGE_NAME, + "waiting for no graph change condition ..."); + } + } + ASSERT_NE(idx, 100); + + // Graph change since creating the publisher + rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); + ret = rcl_publisher_init( + &pub, node_ptr, ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes), + "/chatter_test_graph_guard_condition_topics", &pub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + { + SCOPED_TRACE("Check guard condition change failed !"); + CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s); + } + + // Graph change since destroying the publisher + ret = rcl_publisher_fini(&pub, node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + { + SCOPED_TRACE("Check guard condition change failed !"); + CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s); + } + + // Graph change since creating the subscription + rcl_subscription_t sub = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options(); + ret = rcl_subscription_init( + &sub, node_ptr, ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes), + "/chatter_test_graph_guard_condition_topics", &sub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + { + SCOPED_TRACE("Check guard condition change failed !"); + CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s); + } + + // Graph change since destroying the subscription + ret = rcl_subscription_fini(&sub, node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + { + SCOPED_TRACE("Check guard condition change failed !"); + CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s); + } + + // Graph change since creating service + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + ret = rcl_service_init( + &service, + node_ptr, + ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes), + "test_graph_guard_condition_service", + &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + { + SCOPED_TRACE("Check guard condition change failed !"); + CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s); + } + + // Graph change since destroy service + ret = rcl_service_fini(&service, node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + { + SCOPED_TRACE("Check guard condition change failed !"); + CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s); + } + + // Graph change since creating client + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options = rcl_client_get_default_options(); + ret = rcl_client_init( + &client, + node_ptr, + ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes), + "test_graph_guard_condition_service", + &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + { + SCOPED_TRACE("Check guard condition change failed !"); + CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s); + } + + // Graph change since destroying client + ret = rcl_client_fini(&client, node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + { + SCOPED_TRACE("Check guard condition change failed !"); + CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s); + } + + // Graph change since adding new node + rcl_node_t node_new = rcl_get_zero_initialized_node(); + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(&node_new, "test_graph2", "", context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + { + SCOPED_TRACE("Check guard condition change failed !"); + CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_3s); + } + + // Graph change since destroying new node + ret = rcl_node_fini(&node_new); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + { + SCOPED_TRACE("Check guard condition change failed !"); + CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK, timeout_1s); + } + + // Should not get graph change if no change + { + SCOPED_TRACE("Check guard condition change failed !"); + CHECK_GUARD_CONDITION_CHANGE(RCL_RET_TIMEOUT, timeout_1s); + } +} + +/* Test the rcl_service_server_is_available function. + */ +TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_rcl_service_server_is_available) { + rcl_ret_t ret; + // First create a client which will be used to call the function. + rcl_client_t client = rcl_get_zero_initialized_client(); + auto ts = ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + const char * service_name = "/service_test_rcl_service_server_is_available"; + rcl_client_options_t client_options = rcl_client_get_default_options(); + ret = rcl_client_init(&client, this->node_ptr, ts, service_name, &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + // Check, knowing there is no service server (created by us at least). + bool is_available; + ret = rcl_service_server_is_available(this->node_ptr, &client, &is_available); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_FALSE(is_available); + // Setup function to wait for service state to change using graph guard condition. + const rcl_guard_condition_t * graph_guard_condition = + rcl_node_get_graph_guard_condition(this->node_ptr); + ASSERT_NE(nullptr, graph_guard_condition) << rcl_get_error_string().str; + auto wait_for_service_state_to_change = [this, &graph_guard_condition, &client]( + bool expected_state, + bool & is_available) + { + is_available = false; + auto start = std::chrono::steady_clock::now(); + auto end = start + std::chrono::seconds(10); + while (std::chrono::steady_clock::now() < end) { + // We wait multiple times in case other graph changes are occurring simultaneously. + std::chrono::nanoseconds time_left = end - start; + std::chrono::nanoseconds time_to_sleep = time_left; + std::chrono::seconds min_sleep(1); + if (time_to_sleep > min_sleep) { + time_to_sleep = min_sleep; + } + rcl_ret_t ret = rcl_wait_set_clear(this->wait_set_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition, NULL); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + RCUTILS_LOG_INFO_NAMED( + ROS_PACKAGE_NAME, + "waiting up to '%s' nanoseconds for graph changes", + std::to_string(time_to_sleep.count()).c_str()); + ret = rcl_wait(this->wait_set_ptr, time_to_sleep.count()); + if (ret == RCL_RET_TIMEOUT) { + if (!is_connext) { + // TODO(wjwwood): + // Connext has a race condition which can cause the graph guard + // condition to wake up due to the necessary topics going away, + // but afterwards rcl_service_server_is_available() still does + // not reflect that the service is "no longer available". + // The result is that some tests are flaky unless you not only + // check right after a graph change but again in the future where + // rcl_service_server_is_available() eventually reports the + // service is no longer there. This condition can be removed and + // we can always continue when we get RCL_RET_TIMEOUT once that + // is fixed. + continue; + } + } else { + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + ret = rcl_service_server_is_available(this->node_ptr, &client, &is_available); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + if (is_available == expected_state) { + break; + } + } + }; + { + // Create the service server. + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + ret = rcl_service_init(&service, this->node_ptr, ts, service_name, &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + // Wait for and then assert that it is available. + wait_for_service_state_to_change(true, is_available); + ASSERT_TRUE(is_available); + } + // Assert the state goes back to "not available" after the service is removed. + wait_for_service_state_to_change(false, is_available); + ASSERT_FALSE(is_available); +} + +/* Test passing invalid params to server_is_available + */ +TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_bad_server_available) { + // Create a client which will be used to call the function. + rcl_client_t client = rcl_get_zero_initialized_client(); + auto ts = ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + const char * service_name = "/service_test_rcl_service_server_is_available"; + rcl_client_options_t client_options = rcl_client_get_default_options(); + rcl_ret_t ret = rcl_client_init(&client, this->node_ptr, ts, service_name, &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + // Check, knowing there is no service server (created by us at least). + bool is_available; + ret = rcl_service_server_is_available(this->node_ptr, &client, &is_available); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_FALSE(is_available); + + ret = rcl_service_server_is_available(nullptr, &client, &is_available); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + rcl_node_t not_init_node = rcl_get_zero_initialized_node(); + ret = rcl_service_server_is_available(¬_init_node, &client, &is_available); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); +} + +/* Test passing invalid params to get_node_names functions + */ +TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_bad_get_node_names) { + rcutils_string_array_t node_names = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces = rcutils_get_zero_initialized_string_array(); + + rcutils_string_array_t node_names_2 = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_namespaces_2 = rcutils_get_zero_initialized_string_array(); + rcutils_string_array_t node_enclaves = rcutils_get_zero_initialized_string_array(); + rcl_ret_t ret = RCL_RET_OK; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcutils_string_array_fini(&node_names); + EXPECT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_string_array_fini(&node_namespaces); + EXPECT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_string_array_fini(&node_names_2); + EXPECT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_string_array_fini(&node_namespaces_2); + EXPECT_EQ(RCUTILS_RET_OK, ret); + ret = rcutils_string_array_fini(&node_enclaves); + EXPECT_EQ(RCUTILS_RET_OK, ret); + }); + rcl_allocator_t allocator = rcl_get_default_allocator(); + + // Invalid nullptr as node + ret = rcl_get_node_names(nullptr, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + ret = rcl_get_node_names_with_enclaves( + nullptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + + // Invalid not init node + rcl_node_t not_init_node = rcl_get_zero_initialized_node(); + ret = rcl_get_node_names(¬_init_node, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + ret = rcl_get_node_names_with_enclaves( + ¬_init_node, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + + // Invalid nullptr as node names output + ret = rcl_get_node_names(this->node_ptr, allocator, nullptr, &node_namespaces); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, nullptr, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + // Invalid nullptr as node_namespaces output + ret = rcl_get_node_names(this->node_ptr, allocator, &node_names, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, nullptr, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + // Invalid nullptr as node_enclaves output + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + // Invalid node_names previously init (size is set) + node_names.size = 1; + ret = rcl_get_node_names(this->node_ptr, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + node_names.size = 0; + + // Invalid node_names previously init (size is zero, but internal structure size is 1) + ret = rcutils_string_array_init(&node_names, 1, &allocator); + EXPECT_EQ(RCL_RET_OK, ret); + node_names.size = 0; + ret = rcl_get_node_names(this->node_ptr, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + node_names.size = 1; + ret = rcutils_string_array_fini(&node_names); + EXPECT_EQ(RCL_RET_OK, ret); + + // Invalid node_namespaces previously init (size is set) + node_namespaces.size = 1; + ret = rcl_get_node_names(this->node_ptr, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + node_namespaces.size = 0; + + // Invalid node_namespaces previously init (size is zero, but internal structure size is 1) + ret = rcutils_string_array_init(&node_namespaces, 1, &allocator); + EXPECT_EQ(RCL_RET_OK, ret); + node_namespaces.size = 0; + ret = rcl_get_node_names(this->node_ptr, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + node_namespaces.size = 1; + ret = rcutils_string_array_fini(&node_namespaces); + EXPECT_EQ(RCL_RET_OK, ret); + + // Invalid node_enclaves previously init (size is set) + node_enclaves.size = 1; + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + node_enclaves.size = 0; + + // Invalid node_enclave previously init (size is zero, but internal structure size is 1) + ret = rcutils_string_array_init(&node_enclaves, 1, &allocator); + EXPECT_EQ(RCL_RET_OK, ret); + node_enclaves.size = 0; + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names, &node_namespaces, &node_enclaves); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + node_enclaves.size = 1; + ret = rcutils_string_array_fini(&node_enclaves); + EXPECT_EQ(RCL_RET_OK, ret); + + // Expected usage + ret = rcl_get_node_names(this->node_ptr, allocator, &node_names, &node_namespaces); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_get_node_names_with_enclaves( + this->node_ptr, allocator, &node_names_2, &node_namespaces_2, &node_enclaves); + EXPECT_EQ(RCL_RET_OK, ret); +} diff --git a/test/rcl/test_guard_condition.cpp b/test/rcl/test_guard_condition.cpp new file mode 100644 index 0000000..48d4ede --- /dev/null +++ b/test/rcl/test_guard_condition.cpp @@ -0,0 +1,253 @@ +// 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 "rcl/rcl.h" +#include "rcl/guard_condition.h" + +#include "./failing_allocator_functions.hpp" +#include "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp" +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" + +#include "rmw/rmw.h" + +#include "../mocking_utils/patch.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +using osrf_testing_tools_cpp::memory_tools::on_unexpected_malloc; +using osrf_testing_tools_cpp::memory_tools::on_unexpected_realloc; +using osrf_testing_tools_cpp::memory_tools::on_unexpected_calloc; +using osrf_testing_tools_cpp::memory_tools::on_unexpected_free; + +class CLASSNAME (TestGuardConditionFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + void SetUp() + { + osrf_testing_tools_cpp::memory_tools::initialize(); + on_unexpected_malloc([]() {ASSERT_FALSE(true) << "UNEXPECTED MALLOC";}); + on_unexpected_realloc([]() {ASSERT_FALSE(true) << "UNEXPECTED REALLOC";}); + on_unexpected_calloc([]() {ASSERT_FALSE(true) << "UNEXPECTED CALLOC";}); + on_unexpected_free([]() {ASSERT_FALSE(true) << "UNEXPECTED FREE";}); + } + + void TearDown() + { + osrf_testing_tools_cpp::memory_tools::uninitialize(); + } +}; + +/* Tests the guard condition accessors, i.e. rcl_guard_condition_get_* functions. + */ +TEST_F( + CLASSNAME(TestGuardConditionFixture, RMW_IMPLEMENTATION), test_rcl_guard_condition_accessors) { + osrf_testing_tools_cpp::memory_tools::enable_monitoring_in_all_threads(); + + rcl_ret_t ret; + + // Initialize rcl with rcl_init(). + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + osrf_testing_tools_cpp::memory_tools::disable_monitoring_in_all_threads(); + ASSERT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)); + }); + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // Setup automatic rcl_shutdown() + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + osrf_testing_tools_cpp::memory_tools::disable_monitoring_in_all_threads(); + ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context)); + ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context)); + }); + + // Create a zero initialized guard_condition (but not initialized). + rcl_guard_condition_t zero_guard_condition = rcl_get_zero_initialized_guard_condition(); + + // Create a normal guard_condition. + rcl_guard_condition_options_t default_options = rcl_guard_condition_get_default_options(); + rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition(); + ret = rcl_guard_condition_init(&guard_condition, &context, default_options); + ASSERT_EQ(RCL_RET_OK, ret); + // Setup automatic finalization of guard condition. + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + osrf_testing_tools_cpp::memory_tools::disable_monitoring_in_all_threads(); + rcl_ret_t ret = rcl_guard_condition_fini(&guard_condition); + EXPECT_EQ(RCL_RET_OK, ret); + }); + + // Test rcl_guard_condition_get_options(). + const rcl_guard_condition_options_t * actual_options; + actual_options = rcl_guard_condition_get_options(nullptr); + EXPECT_EQ(nullptr, actual_options); + rcl_reset_error(); + actual_options = rcl_guard_condition_get_options(&zero_guard_condition); + EXPECT_EQ(nullptr, actual_options); + rcl_reset_error(); + EXPECT_NO_MEMORY_OPERATIONS( + { + actual_options = rcl_guard_condition_get_options(&guard_condition); + }); + EXPECT_NE(nullptr, actual_options); + if (actual_options) { + EXPECT_EQ(default_options.allocator.allocate, actual_options->allocator.allocate); + } + // Test rcl_guard_condition_get_rmw_handle(). + rmw_guard_condition_t * gc_handle; + gc_handle = rcl_guard_condition_get_rmw_handle(nullptr); + EXPECT_EQ(nullptr, gc_handle); + rcl_reset_error(); + gc_handle = rcl_guard_condition_get_rmw_handle(&zero_guard_condition); + EXPECT_EQ(nullptr, gc_handle); + rcl_reset_error(); + EXPECT_NO_MEMORY_OPERATIONS( + { + gc_handle = rcl_guard_condition_get_rmw_handle(&guard_condition); + }); + EXPECT_NE(nullptr, gc_handle); +} + +/* Tests the guard condition life cycle, including rcl_guard_condition_init/fini(). + */ +TEST_F( + CLASSNAME(TestGuardConditionFixture, RMW_IMPLEMENTATION), test_rcl_guard_condition_life_cycle) { + rcl_ret_t ret; + rcl_context_t context = rcl_get_zero_initialized_context(); + rcl_guard_condition_t guard_condition = rcl_get_zero_initialized_guard_condition(); + rcl_guard_condition_options_t default_options = rcl_guard_condition_get_default_options(); + // Trying to init before rcl_init() should fail. + ret = rcl_guard_condition_init(&guard_condition, &context, default_options); + ASSERT_EQ(RCL_RET_NOT_INIT, ret) << "Expected RCL_RET_NOT_INIT"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Initialize rcl with rcl_init(). + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + osrf_testing_tools_cpp::memory_tools::disable_monitoring_in_all_threads(); + ASSERT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)); + }); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context)); + ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context)); + }); + // Try invalid arguments. + ret = rcl_guard_condition_init(nullptr, &context, default_options); + ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Now with nullptr for context. + ret = rcl_guard_condition_init(&guard_condition, nullptr, default_options); + ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Try with invalid allocator. + rcl_guard_condition_options_t options_with_invalid_allocator = + rcl_guard_condition_get_default_options(); + options_with_invalid_allocator.allocator.allocate = nullptr; + options_with_invalid_allocator.allocator.deallocate = nullptr; + options_with_invalid_allocator.allocator.reallocate = nullptr; + ret = rcl_guard_condition_init(&guard_condition, &context, options_with_invalid_allocator); + ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Try with failing allocator. + rcl_guard_condition_options_t options_with_failing_allocator = + rcl_guard_condition_get_default_options(); + options_with_failing_allocator.allocator.allocate = failing_malloc; + options_with_failing_allocator.allocator.reallocate = failing_realloc; + options_with_failing_allocator.allocator.zero_allocate = failing_calloc; + ret = rcl_guard_condition_init(&guard_condition, &context, options_with_failing_allocator); + ASSERT_EQ(RCL_RET_BAD_ALLOC, ret) << "Expected RCL_RET_BAD_ALLOC"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Try init but force an internal error. + { + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rmw_create_guard_condition, "internal error", nullptr); + ret = rcl_guard_condition_init(&guard_condition, &context, default_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + + // Try fini with invalid arguments. + ret = rcl_guard_condition_fini(nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Try fini with an uninitialized guard_condition. + ret = rcl_guard_condition_fini(&guard_condition); + EXPECT_EQ(RCL_RET_OK, ret); + // Try a normal init and fini. + ret = rcl_guard_condition_init(&guard_condition, &context, default_options); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_guard_condition_fini(&guard_condition); + EXPECT_EQ(RCL_RET_OK, ret); + // Try normal init and fini, but force an internal error on first try. + { + auto mock = mocking_utils::inject_on_return( + "lib:rcl", rmw_destroy_guard_condition, RMW_RET_ERROR); + ret = rcl_guard_condition_init(&guard_condition, &context, default_options); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_guard_condition_fini(&guard_condition); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + ret = rcl_guard_condition_fini(&guard_condition); + EXPECT_EQ(RCL_RET_OK, ret); + // Try repeated init and fini calls. + ret = rcl_guard_condition_init(&guard_condition, &context, default_options); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_guard_condition_init(&guard_condition, &context, default_options); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << "Expected RCL_RET_ALREADY_INIT"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + ret = rcl_guard_condition_fini(&guard_condition); + EXPECT_EQ(RCL_RET_OK, ret); + rcl_reset_error(); + ret = rcl_guard_condition_fini(&guard_condition); + EXPECT_EQ(RCL_RET_OK, ret); + rcl_reset_error(); +} + +/* Tests trigger_guard_condition with bad arguments + */ +TEST_F( + CLASSNAME(TestGuardConditionFixture, RMW_IMPLEMENTATION), test_rcl_guard_condition_bad_arg) { + rcl_guard_condition_t zero_guard_condition = rcl_get_zero_initialized_guard_condition(); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_trigger_guard_condition(nullptr)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_trigger_guard_condition(&zero_guard_condition)); + rcl_reset_error(); +} diff --git a/test/rcl/test_info_by_topic.cpp b/test/rcl/test_info_by_topic.cpp new file mode 100644 index 0000000..3ce4385 --- /dev/null +++ b/test/rcl/test_info_by_topic.cpp @@ -0,0 +1,411 @@ +// 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 _WIN32 +# pragma GCC diagnostic ignored "-Wmissing-field-initializers" +#endif + +#include + +#include +#include + +#include "rcl/error_handling.h" +#include "rcl/graph.h" +#include "rcl/rcl.h" + +#include "rmw/topic_endpoint_info_array.h" +#include "rmw/error_handling.h" +#include "wait_for_entity_helpers.hpp" + +#include "test_msgs/msg/strings.h" +#include "rosidl_runtime_c/string_functions.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestInfoByTopicFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t old_context; + rcl_context_t context; + rcl_node_t old_node; + rcl_node_t node; + const char * test_graph_node_name = "test_graph_node"; + rmw_topic_endpoint_info_array_t topic_endpoint_info_array; + const char * const topic_name = "valid_topic_name"; + + void SetUp() + { + rcl_ret_t ret; + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->old_context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &this->old_context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->old_node = rcl_get_zero_initialized_node(); + const char * old_name = "old_node_name"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(&this->old_node, old_name, "", &this->old_context, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(&this->old_context); // after this, the old_node should be invalid + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + this->context = rcl_get_zero_initialized_context(); + + ret = rcl_init(0, nullptr, &init_options, &this->context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + this->node = rcl_get_zero_initialized_node(); + const char * name = "test_graph_node"; + ret = rcl_node_init(&this->node, name, "", &this->context, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret; + ret = rcl_node_fini(&this->old_node); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_node_fini(&this->node); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_shutdown(&this->context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(&this->context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // old_context was supposed to have been shutdown already during SetUp() + if (rcl_context_is_valid(&this->old_context)) { + ret = rcl_shutdown(&this->old_context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + ret = rcl_context_fini(&this->old_context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void assert_qos_equality( + rmw_qos_profile_t qos_profile1, rmw_qos_profile_t qos_profile2, + bool is_publisher) + { + EXPECT_EQ(qos_profile1.deadline.sec, qos_profile2.deadline.sec); + EXPECT_EQ(qos_profile1.deadline.nsec, qos_profile2.deadline.nsec); + if (is_publisher) { + EXPECT_EQ(qos_profile1.lifespan.sec, qos_profile2.lifespan.sec); + EXPECT_EQ(qos_profile1.lifespan.nsec, qos_profile2.lifespan.nsec); + } + EXPECT_EQ(qos_profile1.reliability, qos_profile2.reliability); + EXPECT_EQ(qos_profile1.liveliness, qos_profile2.liveliness); + EXPECT_EQ( + qos_profile1.liveliness_lease_duration.sec, + qos_profile2.liveliness_lease_duration.sec); + EXPECT_EQ( + qos_profile1.liveliness_lease_duration.nsec, + qos_profile2.liveliness_lease_duration.nsec); + EXPECT_EQ(qos_profile1.durability, qos_profile2.durability); + } +}; + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F( + CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_info_by_topic_null_node) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_publishers_info_by_topic( + nullptr, &allocator, this->topic_name, false, + &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F( + CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriptions_info_by_topic_null_node) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_subscriptions_info_by_topic( + nullptr, &allocator, this->topic_name, false, + &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F( + CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_info_by_topic_invalid_node) +{ + // this->old_node is an invalid node. + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_publishers_info_by_topic( + &this->old_node, &allocator, this->topic_name, false, + &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F( + CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriptions_info_by_topic_invalid_node) +{ + // this->old_node is an invalid node. + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_subscriptions_info_by_topic( + &this->old_node, &allocator, this->topic_name, false, + &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F( + CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_info_by_topic_null_allocator) +{ + const auto ret = rcl_get_publishers_info_by_topic( + &this->node, nullptr, this->topic_name, false, + &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F( + CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriptions_info_by_topic_null_allocator) +{ + const auto ret = rcl_get_subscriptions_info_by_topic( + &this->node, nullptr, this->topic_name, false, + &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F( + CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_info_by_topic_null_topic) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_publishers_info_by_topic( + &this->node, &allocator, nullptr, false, &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F( + CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriptions_info_by_topic_null_topic) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_subscriptions_info_by_topic( + &this->node, &allocator, nullptr, false, &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F( + CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_info_by_topic_null_participants) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_publishers_info_by_topic( + &this->node, &allocator, this->topic_name, false, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F( + CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriptions_info_by_topic_null_participants) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_subscriptions_info_by_topic( + &this->node, &allocator, this->topic_name, false, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F( + CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_info_by_topic_invalid_participants) +{ + // topic_endpoint_info_array is invalid because it is expected to be zero initialized + // and the info_array variable inside it is expected to be null. + this->topic_endpoint_info_array.info_array = new rmw_topic_endpoint_info_t(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + delete this->topic_endpoint_info_array.info_array; + }); + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_publishers_info_by_topic( + &this->node, &allocator, this->topic_name, false, + &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_ERROR, ret); +} + +/* + * This does not test content of the response. + * It only tests if the return code is the one expected. + */ +TEST_F( + CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_subscriptions_info_by_topic_invalid_participants) +{ + // topic_endpoint_info_array is invalid because it is expected to be zero initialized + // and the info_array variable inside it is expected to be null. + this->topic_endpoint_info_array.info_array = new rmw_topic_endpoint_info_t(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + delete this->topic_endpoint_info_array.info_array; + }); + rcl_allocator_t allocator = rcl_get_default_allocator(); + const auto ret = rcl_get_subscriptions_info_by_topic( + &this->node, &allocator, this->topic_name, false, + &this->topic_endpoint_info_array); + EXPECT_EQ(RCL_RET_ERROR, ret); +} + +TEST_F( + CLASSNAME(TestInfoByTopicFixture, RMW_IMPLEMENTATION), + test_rcl_get_publishers_subscription_info_by_topic) +{ + rmw_qos_profile_t default_qos_profile = rmw_qos_profile_system_default; + default_qos_profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + default_qos_profile.depth = 0; + default_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + default_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; + default_qos_profile.lifespan = {10, 0}; + default_qos_profile.deadline = {11, 0}; + default_qos_profile.liveliness_lease_duration = {20, 0}; + default_qos_profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + + rcl_ret_t ret; + const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + rcl_allocator_t allocator = rcl_get_default_allocator(); + + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + publisher_options.qos = default_qos_profile; + ret = rcl_publisher_init( + &publisher, + &this->node, + ts, + this->topic_name, + &publisher_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + subscription_options.qos = default_qos_profile; + ret = rcl_subscription_init( + &subscription, + &this->node, + ts, + this->topic_name, + &subscription_options); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + const std::string fqdn = std::string("/") + this->topic_name; + // Wait until GraphCache publishers are updated + bool success = false; + ret = rcl_wait_for_publishers( + &this->node, &allocator, fqdn.c_str(), 1u, RCUTILS_S_TO_NS(1), &success); + ASSERT_EQ(ret, RCL_RET_OK); + ASSERT_TRUE(success); + // Get publishers info by topic + rmw_topic_endpoint_info_array_t topic_endpoint_info_array_pub = + rmw_get_zero_initialized_topic_endpoint_info_array(); + ret = rcl_get_publishers_info_by_topic( + &this->node, &allocator, fqdn.c_str(), false, + &topic_endpoint_info_array_pub); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ASSERT_EQ(topic_endpoint_info_array_pub.size, 1u) << "Expected one publisher"; + rmw_topic_endpoint_info_t topic_endpoint_info_pub = topic_endpoint_info_array_pub.info_array[0]; + EXPECT_STREQ(topic_endpoint_info_pub.node_name, this->test_graph_node_name); + EXPECT_STREQ(topic_endpoint_info_pub.node_namespace, "/"); + EXPECT_STREQ(topic_endpoint_info_pub.topic_type, "test_msgs/msg/Strings"); + assert_qos_equality(topic_endpoint_info_pub.qos_profile, default_qos_profile, true); + + // Wait until GraphCache subcribers are updated + success = false; + ret = rcl_wait_for_subscribers( + &this->node, &allocator, fqdn.c_str(), 1u, RCUTILS_S_TO_NS(1), &success); + ASSERT_EQ(ret, RCL_RET_OK); + ASSERT_TRUE(success); + // Get subscribers info by topic + rmw_topic_endpoint_info_array_t topic_endpoint_info_array_sub = + rmw_get_zero_initialized_topic_endpoint_info_array(); + ret = rcl_get_subscriptions_info_by_topic( + &this->node, &allocator, fqdn.c_str(), false, + &topic_endpoint_info_array_sub); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ASSERT_EQ(topic_endpoint_info_array_sub.size, 1u) << "Expected one subscription"; + rmw_topic_endpoint_info_t topic_endpoint_info_sub = topic_endpoint_info_array_sub.info_array[0]; + EXPECT_STREQ(topic_endpoint_info_sub.node_name, this->test_graph_node_name); + EXPECT_STREQ(topic_endpoint_info_sub.node_namespace, "/"); + EXPECT_STREQ(topic_endpoint_info_sub.topic_type, "test_msgs/msg/Strings"); + assert_qos_equality(topic_endpoint_info_sub.qos_profile, default_qos_profile, false); + + // clean up + rmw_ret_t rmw_ret = + rmw_topic_endpoint_info_array_fini(&topic_endpoint_info_array_pub, &allocator); + EXPECT_EQ(rmw_ret, RMW_RET_OK) << rmw_get_error_string().str; + rmw_ret = rmw_topic_endpoint_info_array_fini(&topic_endpoint_info_array_sub, &allocator); + EXPECT_EQ(rmw_ret, RMW_RET_OK) << rmw_get_error_string().str; + ret = rcl_subscription_fini(&subscription, &this->node); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + ret = rcl_publisher_fini(&publisher, &this->node); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; +} diff --git a/test/rcl/test_init.cpp b/test/rcl/test_init.cpp new file mode 100644 index 0000000..72913c2 --- /dev/null +++ b/test/rcl/test_init.cpp @@ -0,0 +1,576 @@ +// Copyright 2015 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 "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp" +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/arguments.h" +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl/security.h" +#include "rcutils/env.h" +#include "rcutils/format_string.h" +#include "rcutils/snprintf.h" +#include "rcutils/testing/fault_injection.h" + +#include "rmw/rmw.h" + +#include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" +#include "../src/rcl/init_options_impl.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +using osrf_testing_tools_cpp::memory_tools::on_unexpected_malloc; +using osrf_testing_tools_cpp::memory_tools::on_unexpected_realloc; +using osrf_testing_tools_cpp::memory_tools::on_unexpected_calloc; +using osrf_testing_tools_cpp::memory_tools::on_unexpected_free; + +class CLASSNAME (TestRCLFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + void SetUp() + { + osrf_testing_tools_cpp::memory_tools::initialize(); + on_unexpected_malloc([]() {ADD_FAILURE() << "UNEXPECTED MALLOC";}); + on_unexpected_realloc([]() {ADD_FAILURE() << "UNEXPECTED REALLOC";}); + on_unexpected_free([]() {ADD_FAILURE() << "UNEXPECTED FREE";}); + } + + void TearDown() + { + osrf_testing_tools_cpp::memory_tools::uninitialize(); + } +}; + +struct FakeTestArgv +{ + FakeTestArgv() + : allocator(rcl_get_default_allocator()), argc(2) + { + this->argv = + static_cast(allocator.allocate(2 * sizeof(char *), allocator.state)); + if (!this->argv) { + throw std::bad_alloc(); + } + this->argv[0] = rcutils_format_string(allocator, "%s", "foo"); + if (!this->argv[0]) { + allocator.deallocate(this->argv, allocator.state); + throw std::bad_alloc(); + } + this->argv[1] = rcutils_format_string(allocator, "%s", "bar"); + if (!this->argv[1]) { + allocator.deallocate(this->argv[0], allocator.state); + allocator.deallocate(this->argv, allocator.state); + throw std::bad_alloc(); + } + } + + ~FakeTestArgv() + { + if (this->argv) { + if (this->argc > 0) { + size_t unsigned_argc = this->argc; + for (size_t i = 0; i < unsigned_argc; ++i) { + allocator.deallocate(this->argv[i], allocator.state); + } + } + } + allocator.deallocate(this->argv, allocator.state); + } + + rcl_allocator_t allocator; + int argc; + char ** argv; + +private: + FakeTestArgv(const FakeTestArgv &) = delete; +}; + +/* Tests rcl_init_options_init() and rcl_init_options_fini() functions. + */ +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_options_init) { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + + // fini a not empty options + rcl_ret_t ret = rcl_init_options_fini(&init_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Expected usage + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + + // Already init + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // nullptr + ret = rcl_init_options_init(nullptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // nullptr + ret = rcl_init_options_fini(nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +/* Tests calling rcl_init() with invalid arguments fails. + */ +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_invalid_arguments) { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + + { + // If argc is not 0, but argv is, it should be an invalid argument. + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(42, nullptr, &init_options, &context); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); + } + { + // If argc is not 0, argv is not null but contains one, it should be an invalid argument. + rcl_context_t context = rcl_get_zero_initialized_context(); + const char * null_args[] = {"some-arg", nullptr}; + ret = rcl_init(2, null_args, &init_options, &context); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); + } + { + // If argc is less than 1, argv is not null, it should be an invalid argument. + rcl_context_t context = rcl_get_zero_initialized_context(); + const char * some_args[] = {"some-arg"}; + ret = rcl_init(0, some_args, &init_options, &context); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); + } + { + // If an invalid ROS arg is given, init should fail. + rcl_context_t context = rcl_get_zero_initialized_context(); + const char * bad_remap_args[] = { + "some-arg", RCL_ROS_ARGS_FLAG, RCL_REMAP_FLAG, "name:="}; + const size_t argc = sizeof(bad_remap_args) / sizeof(const char *); + ret = rcl_init(argc, bad_remap_args, &init_options, &context); + EXPECT_EQ(RCL_RET_INVALID_ROS_ARGS, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); + } + { + // If an invalid enclave is given, init should fail. + rcl_context_t context = rcl_get_zero_initialized_context(); + const char * bad_enclave_args[] = { + "some-arg", RCL_ROS_ARGS_FLAG, RCL_ENCLAVE_FLAG, "1foo"}; + const size_t argc = sizeof(bad_enclave_args) / sizeof(const char *); + ret = rcl_init(argc, bad_enclave_args, &init_options, &context); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); + } + { + // If security keystore is invalid, init should fail. + ASSERT_TRUE(rcutils_set_env(ROS_SECURITY_ENABLE_VAR_NAME, "true")); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_TRUE(rcutils_set_env(ROS_SECURITY_ENABLE_VAR_NAME, "")); + }); + ASSERT_TRUE(rcutils_set_env(ROS_SECURITY_STRATEGY_VAR_NAME, "Enforce")); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_TRUE(rcutils_set_env(ROS_SECURITY_STRATEGY_VAR_NAME, "")); + }); + ASSERT_TRUE(rcutils_set_env(ROS_SECURITY_KEYSTORE_VAR_NAME, "/not/a/real/secure/root")); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_TRUE(rcutils_set_env(ROS_SECURITY_KEYSTORE_VAR_NAME, "")); + }); + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &context); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); + } + { + // If either the allocate or deallocate function pointers are not set, + // it should be invalid arg. + rcl_context_t context = rcl_get_zero_initialized_context(); + rcl_allocator_t allocator = init_options.impl->allocator; + init_options.impl->allocator = + (rcl_allocator_t)rcutils_get_zero_initialized_allocator(); + ret = rcl_init(0, nullptr, &init_options, &context); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + init_options.impl->allocator = allocator; + ASSERT_FALSE(rcl_context_is_valid(&context)); + } + { + // If the malloc call fails (with some valid arguments to copy), + // it should be a bad alloc. + FakeTestArgv test_args; + rcl_allocator_t allocator = init_options.impl->allocator; + init_options.impl->allocator = get_failing_allocator(); + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(test_args.argc, test_args.argv, &init_options, &context); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + rcl_reset_error(); + init_options.impl->allocator = allocator; + ASSERT_FALSE(rcl_context_is_valid(&context)); + } +} + +/* Tests the rcl_init() and rcl_shutdown() functions. + */ +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_and_shutdown) { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + rcl_context_t context = rcl_get_zero_initialized_context(); + // A shutdown before an init should fail. + ret = rcl_shutdown(&context); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); + // If argc is 0 and argv is nullptr and the allocator is valid, it should succeed. + ret = rcl_init(0, nullptr, &init_options, &context); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_TRUE(rcl_context_is_valid(&context)); + // Then shutdown should work. + ret = rcl_shutdown(&context); + EXPECT_EQ(ret, RCL_RET_OK); + ASSERT_FALSE(rcl_context_is_valid(&context)); + ret = rcl_context_fini(&context); + EXPECT_EQ(ret, RCL_RET_OK); + context = rcl_get_zero_initialized_context(); + // Valid argc/argv values and a valid allocator should succeed. + { + FakeTestArgv test_args; + ret = rcl_init(test_args.argc, test_args.argv, &init_options, &context); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_TRUE(rcl_context_is_valid(&context)); + } + // Then shutdown should work. + ret = rcl_shutdown(&context); + EXPECT_EQ(ret, RCL_RET_OK); + ASSERT_FALSE(rcl_context_is_valid(&context)); + // Then a repeated shutdown should fail. + ret = rcl_shutdown(&context); + EXPECT_EQ(ret, RCL_RET_ALREADY_SHUTDOWN); + ASSERT_FALSE(rcl_context_is_valid(&context)); + rcl_reset_error(); + ret = rcl_context_fini(&context); + EXPECT_EQ(ret, RCL_RET_OK); + context = rcl_get_zero_initialized_context(); + // A repeat call to shutdown should not work. + ret = rcl_shutdown(&context); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + ASSERT_FALSE(rcl_context_is_valid(&context)); + // Repeat, but valid, calls to rcl_init() should fail. + { + FakeTestArgv test_args; + ret = rcl_init(test_args.argc, test_args.argv, &init_options, &context); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_TRUE(rcl_context_is_valid(&context)); + ret = rcl_init(test_args.argc, test_args.argv, &init_options, &context); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret); + rcl_reset_error(); + ASSERT_TRUE(rcl_context_is_valid(&context)); + } + // But shutdown should still work. + ret = rcl_shutdown(&context); + EXPECT_EQ(ret, RCL_RET_OK); + ASSERT_FALSE(rcl_context_is_valid(&context)); + ret = rcl_context_fini(&context); + EXPECT_EQ(ret, RCL_RET_OK); + context = rcl_get_zero_initialized_context(); +} + +/* Tests rcl_init() deals with internal errors correctly. + */ +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_internal_error) { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + FakeTestArgv test_args; + rcl_context_t context = rcl_get_zero_initialized_context(); + + { + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rmw_init, "internal error", RMW_RET_ERROR); + ret = rcl_init(test_args.argc, test_args.argv, &init_options, &context); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + EXPECT_FALSE(rcl_context_is_valid(&context)); + } + + RCUTILS_FAULT_INJECTION_TEST( + { + ret = rcl_init(test_args.argc, test_args.argv, &init_options, &context); + + int64_t count = rcutils_fault_injection_get_count(); + rcutils_fault_injection_set_count(RCUTILS_FAULT_INJECTION_NEVER_FAIL); + + if (RCL_RET_OK == ret) { + ASSERT_TRUE(rcl_context_is_valid(&context)); + EXPECT_EQ(RCL_RET_OK, rcl_shutdown(&context)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_context_fini(&context)) << rcl_get_error_string().str; + } else { + ASSERT_FALSE(rcl_context_is_valid(&context)); + rcl_reset_error(); + } + + rcutils_fault_injection_set_count(count); + }); +} + +/* Tests rcl_shutdown() deals with internal errors correctly. + */ +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_shutdown_internal_error) { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + rcl_context_t context = rcl_get_zero_initialized_context(); + + ret = rcl_init(0, nullptr, &init_options, &context); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_shutdown(&context)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_context_fini(&context)) << rcl_get_error_string().str; + }); + EXPECT_TRUE(rcl_context_is_valid(&context)); + + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rmw_shutdown, "internal error", RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_shutdown(&context)); + rcl_reset_error(); +} + +/* Tests the rcl_get_instance_id() function. + */ +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_get_instance_id) { + rcl_ret_t ret; + rcl_context_t context = rcl_get_zero_initialized_context(); + // Instance id should be 0 before rcl_init(). + EXPECT_EQ(0u, rcl_context_get_instance_id(&context)); + ASSERT_FALSE(rcl_context_is_valid(&context)); + // It should still return 0 after an invalid init. + ret = rcl_init(1, nullptr, nullptr, &context); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_EQ(0u, rcl_context_get_instance_id(&context)); + ASSERT_FALSE(rcl_context_is_valid(&context)); + rcl_reset_error(); + // A non-zero instance id should be returned after a valid init. + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + { + FakeTestArgv test_args; + ret = rcl_init(test_args.argc, test_args.argv, &init_options, &context); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_TRUE(rcl_context_is_valid(&context)); + } + // And it should be allocation free. + uint64_t first_instance_id; + EXPECT_NO_MEMORY_OPERATIONS( + { + first_instance_id = rcl_context_get_instance_id(&context); + }); + EXPECT_NE(0u, first_instance_id); + // Repeat calls should return the same. + EXPECT_EQ(first_instance_id, rcl_context_get_instance_id(&context)); + EXPECT_EQ(true, rcl_context_is_valid(&context)); + // Calling after a shutdown should return 0. + ret = rcl_shutdown(&context); + EXPECT_EQ(ret, RCL_RET_OK); + EXPECT_EQ(0u, rcl_context_get_instance_id(&context)); + ASSERT_FALSE(rcl_context_is_valid(&context)); + ret = rcl_context_fini(&context); + EXPECT_EQ(ret, RCL_RET_OK); + context = rcl_get_zero_initialized_context(); + // It should return a different value after another valid init. + { + FakeTestArgv test_args; + ret = rcl_init(test_args.argc, test_args.argv, &init_options, &context); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_TRUE(rcl_context_is_valid(&context)); + } + EXPECT_NE(0u, rcl_context_get_instance_id(&context)); + EXPECT_NE(first_instance_id, rcl_context_get_instance_id(&context)); + ASSERT_TRUE(rcl_context_is_valid(&context)); + // Shutting down a second time should result in 0 again. + ret = rcl_shutdown(&context); + EXPECT_EQ(ret, RCL_RET_OK); + EXPECT_EQ(0u, rcl_context_get_instance_id(&context)); + ASSERT_FALSE(rcl_context_is_valid(&context)); + ret = rcl_context_fini(&context); + EXPECT_EQ(ret, RCL_RET_OK); +} + +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_options_access) { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_init_options_t not_ini_init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + + rmw_init_options_t * options = rcl_init_options_get_rmw_init_options(&init_options); + ASSERT_NE(nullptr, options); + EXPECT_EQ(0u, options->instance_id); + EXPECT_EQ(nullptr, options->impl); + EXPECT_EQ(NULL, rcl_init_options_get_rmw_init_options(nullptr)); + EXPECT_EQ(NULL, rcl_init_options_get_rmw_init_options(¬_ini_init_options)); + + const rcl_allocator_t * options_allocator = rcl_init_options_get_allocator(&init_options); + EXPECT_TRUE(rcutils_allocator_is_valid(options_allocator)); + EXPECT_EQ(NULL, rcl_init_options_get_allocator(nullptr)); + EXPECT_EQ(NULL, rcl_init_options_get_allocator(¬_ini_init_options)); + + size_t domain_id; + ret = rcl_init_options_get_domain_id(NULL, &domain_id); + ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_init_options_get_domain_id(¬_ini_init_options, &domain_id); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_init_options_get_domain_id(&init_options, NULL); + ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_init_options_get_domain_id(NULL, NULL); + ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_init_options_set_domain_id(NULL, domain_id); + ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_init_options_set_domain_id(¬_ini_init_options, domain_id); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_init_options_get_domain_id(&init_options, &domain_id); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_DEFAULT_DOMAIN_ID, domain_id); + ret = rcl_init_options_set_domain_id(&init_options, static_cast(0u)); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_init_options_get_domain_id(&init_options, &domain_id); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(0U, domain_id); + + rcl_init_options_t init_options_dst = rcl_get_zero_initialized_init_options(); + + // nullptr copy cases + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_init_options_copy(nullptr, &init_options_dst)); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_init_options_copy(&init_options, nullptr)); + + // Expected usage copy + ASSERT_EQ(RCL_RET_OK, rcl_init_options_copy(&init_options, &init_options_dst)); + ret = rcl_init_options_get_domain_id(&init_options_dst, &domain_id); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(0U, domain_id); + EXPECT_EQ(RCL_RET_ALREADY_INIT, rcl_init_options_copy(&init_options, &init_options_dst)); + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options_dst)); +} + +// Define dummy comparison operators for rcutils_allocator_t type for use with the Mimick Library +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) + +// Tests rcl_init_options_init() mocked to fail +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_mocked_rcl_init_options_ini) { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_init_options_init, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_init_options_init(&init_options, rcl_get_default_allocator())); + rcl_reset_error(); +} + +// Tests rcl_init_options_fini() mocked to fail +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_mocked_rcl_init_options_fini) { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + auto mock = mocking_utils::inject_on_return("lib:rcl", rmw_init_options_fini, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_init_options_fini(&init_options)); + rcl_reset_error(); + auto mock_ok = mocking_utils::inject_on_return("lib:rcl", rmw_init_options_fini, RMW_RET_OK); + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)); +} + +// Mock rcl_init_options_copy to fail +TEST_F(CLASSNAME(TestRCLFixture, RMW_IMPLEMENTATION), test_rcl_init_options_copy_fail_rmw_copy) { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + rcl_init_options_t init_options_dst = rcl_get_zero_initialized_init_options(); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + // dst is in a invalid state after failed copy + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_init_options_fini(&init_options_dst)) << rcl_get_error_string().str; + rcl_reset_error(); + auto mock_ok = mocking_utils::patch_and_return("lib:rcl", rmw_init_options_fini, RMW_RET_OK); + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options_dst)) << rcl_get_error_string().str; + }); + + // rmw_init_options_copy error is logged + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_init_options_copy, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_init_options_copy(&init_options, &init_options_dst)); + rcl_reset_error(); +} diff --git a/test/rcl/test_lexer.cpp b/test/rcl/test_lexer.cpp new file mode 100644 index 0000000..ee2dc1f --- /dev/null +++ b/test/rcl/test_lexer.cpp @@ -0,0 +1,367 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "rcl/lexer.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestLexerFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + void SetUp() + { + } + + void TearDown() + { + } +}; + +// Not using a function so gtest failure output shows the line number where the macro is used +#define EXPECT_LEX(expected_lexeme, expected_text, text) \ + do { \ + rcl_lexeme_t actual_lexeme; \ + size_t length; \ + rcl_ret_t ret = rcl_lexer_analyze(text, &actual_lexeme, &length); \ + ASSERT_EQ(RCL_RET_OK, ret); \ + EXPECT_EQ(expected_lexeme, actual_lexeme); \ + std::string actual_text(text, length); \ + EXPECT_STREQ(expected_text, actual_text.c_str()); \ + } while (false) + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_token_different_endings) +{ + // Things get recognized as tokens whether input ends or non token characters come after them + EXPECT_LEX(RCL_LEXEME_TOKEN, "foo", "foo"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "foo", "foo:"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "foo_", "foo_"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "foo_", "foo_:"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_token_start_char) +{ + // Check full range for starting character + EXPECT_LEX(RCL_LEXEME_TOKEN, "a", "a"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "b", "b"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "c", "c"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "d", "d"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "e", "e"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "f", "f"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "g", "g"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "h", "h"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "i", "i"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "j", "j"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "k", "k"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "l", "l"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "m", "m"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "n", "n"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "o", "o"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "p", "p"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "q", "q"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "r", "r"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "s", "s"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "t", "t"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "u", "u"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "v", "v"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "w", "w"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "x", "x"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "y", "y"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "z", "z"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "A", "A"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "B", "B"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "C", "C"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "D", "D"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "E", "E"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "F", "F"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "G", "G"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "H", "H"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "I", "I"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "J", "J"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "K", "K"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "L", "L"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "M", "M"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "N", "N"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "O", "O"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "P", "P"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "Q", "Q"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "R", "R"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "S", "S"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "T", "T"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "U", "U"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "V", "V"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "W", "W"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "X", "X"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "Y", "Y"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "Z", "Z"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_", "_"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_token_adjacent_ascii) +{ + // Check banned characters adjacent to allowed ones in ASCII + EXPECT_LEX(RCL_LEXEME_NONE, "@", "@"); + EXPECT_LEX(RCL_LEXEME_NONE, "[", "["); + EXPECT_LEX(RCL_LEXEME_NONE, "`", "`"); + EXPECT_LEX(RCL_LEXEME_NONE, "{", "{"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_token_cannot_start_with_digits) +{ + // Tokens cannot start with digits + EXPECT_LEX(RCL_LEXEME_NONE, "0", "0"); + EXPECT_LEX(RCL_LEXEME_NONE, "1", "1"); + EXPECT_LEX(RCL_LEXEME_NONE, "2", "2"); + EXPECT_LEX(RCL_LEXEME_NONE, "3", "3"); + EXPECT_LEX(RCL_LEXEME_NONE, "4", "4"); + EXPECT_LEX(RCL_LEXEME_NONE, "5", "5"); + EXPECT_LEX(RCL_LEXEME_NONE, "6", "6"); + EXPECT_LEX(RCL_LEXEME_NONE, "7", "7"); + EXPECT_LEX(RCL_LEXEME_NONE, "8", "8"); + EXPECT_LEX(RCL_LEXEME_NONE, "9", "9"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_token_underscores) +{ + // Tokens may contain underscores + EXPECT_LEX(RCL_LEXEME_TOKEN, "_abcd", "_abcd"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "abcd_", "abcd_"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "ab_cd", "ab_cd"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_a_b_c_d_", "_a_b_c_d_"); + + // Tokens cannot contain double underscores + EXPECT_LEX(RCL_LEXEME_TOKEN, "_a_", "_a__bcd"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a_", "a__bcd"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "A_", "A__bcd"); + EXPECT_LEX(RCL_LEXEME_NONE, "__a", "__a"); + EXPECT_LEX(RCL_LEXEME_NONE, "__A", "__A"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_token_contain_digits) +{ + // Tokens may contain digits + EXPECT_LEX(RCL_LEXEME_TOKEN, "_0_", "_0_"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_1_", "_1_"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_2_", "_2_"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_3_", "_3_"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_4_", "_4_"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_5_", "_5_"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_6_", "_6_"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_7_", "_7_"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_8_", "_8_"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_9_", "_9_"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a0a", "a0a"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a1a", "a1a"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a2a", "a2a"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a3a", "a3a"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a4a", "a4a"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a5a", "a5a"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a6a", "a6a"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a7a", "a7a"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a8a", "a8a"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a9a", "a9a"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_token_end_with_digits) +{ + // Tokens may end with digits + EXPECT_LEX(RCL_LEXEME_TOKEN, "_0", "_0"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_1", "_1"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_2", "_2"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_3", "_3"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_4", "_4"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_5", "_5"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_6", "_6"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_7", "_7"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_8", "_8"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_9", "_9"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a0", "a0"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a1", "a1"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a2", "a2"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a3", "a3"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a4", "a4"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a5", "a5"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a6", "a6"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a7", "a7"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a8", "a8"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "a9", "a9"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_token_close_to_url_scheme) +{ + // Things that almost look like a url scheme but are actually tokens + EXPECT_LEX(RCL_LEXEME_TOKEN, "ro", "ro"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "ros", "ros"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "ross", "ross"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rosse", "rosse"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rosser", "rosser"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rosserv", "rosserv"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rosservi", "rosservi"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rosservic", "rosservic"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rosservice", "rosservice"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rosservice", "rosservice:"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rosservice", "rosservice:="); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rosservice", "rosservice:/"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rosservice", "rosservice:/a"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rost", "rost"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rosto", "rosto"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rostop", "rostop"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rostopi", "rostopi"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rostopic", "rostopic"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rostopic", "rostopic:"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rostopic", "rostopic:="); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rostopic", "rostopic:/"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "rostopic", "rostopic:/a"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_token_upper_case) +{ + // Tokens may contain uppercase characters + EXPECT_LEX(RCL_LEXEME_TOKEN, "ABC", "ABC"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_DEF", "_DEF"); + EXPECT_LEX(RCL_LEXEME_TOKEN, "_GHI_", "_GHI_"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_url_scheme) +{ + // No text after scheme + EXPECT_LEX(RCL_LEXEME_URL_SERVICE, "rosservice://", "rosservice://"); + EXPECT_LEX(RCL_LEXEME_URL_TOPIC, "rostopic://", "rostopic://"); + + // Some text after scheme + EXPECT_LEX(RCL_LEXEME_URL_SERVICE, "rosservice://", "rosservice://abcd"); + EXPECT_LEX(RCL_LEXEME_URL_SERVICE, "rosservice://", "rosservice:///"); + EXPECT_LEX(RCL_LEXEME_URL_TOPIC, "rostopic://", "rostopic://abcd"); + EXPECT_LEX(RCL_LEXEME_URL_TOPIC, "rostopic://", "rostopic:///"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_backreferences) +{ + // No text after backreference + EXPECT_LEX(RCL_LEXEME_BR1, "\\1", "\\1"); + EXPECT_LEX(RCL_LEXEME_BR2, "\\2", "\\2"); + EXPECT_LEX(RCL_LEXEME_BR3, "\\3", "\\3"); + EXPECT_LEX(RCL_LEXEME_BR4, "\\4", "\\4"); + EXPECT_LEX(RCL_LEXEME_BR5, "\\5", "\\5"); + EXPECT_LEX(RCL_LEXEME_BR6, "\\6", "\\6"); + EXPECT_LEX(RCL_LEXEME_BR7, "\\7", "\\7"); + EXPECT_LEX(RCL_LEXEME_BR8, "\\8", "\\8"); + EXPECT_LEX(RCL_LEXEME_BR9, "\\9", "\\9"); + + // Some text after backreference + EXPECT_LEX(RCL_LEXEME_BR1, "\\1", "\\1a"); + EXPECT_LEX(RCL_LEXEME_BR2, "\\2", "\\2a"); + EXPECT_LEX(RCL_LEXEME_BR3, "\\3", "\\3a"); + EXPECT_LEX(RCL_LEXEME_BR4, "\\4", "\\4a"); + EXPECT_LEX(RCL_LEXEME_BR5, "\\5", "\\5a"); + EXPECT_LEX(RCL_LEXEME_BR6, "\\6", "\\6a"); + EXPECT_LEX(RCL_LEXEME_BR7, "\\7", "\\7a"); + EXPECT_LEX(RCL_LEXEME_BR8, "\\8", "\\8a"); + EXPECT_LEX(RCL_LEXEME_BR9, "\\9", "\\9a"); + + // Not valid backreferences + EXPECT_LEX(RCL_LEXEME_NONE, "\\0", "\\0"); + EXPECT_LEX(RCL_LEXEME_NONE, "\\a", "\\a"); + EXPECT_LEX(RCL_LEXEME_NONE, "\\Z", "\\Z"); + EXPECT_LEX(RCL_LEXEME_NONE, "\\_", "\\_"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_forward_slash) +{ + EXPECT_LEX(RCL_LEXEME_FORWARD_SLASH, "/", "/"); + EXPECT_LEX(RCL_LEXEME_FORWARD_SLASH, "/", "//"); + EXPECT_LEX(RCL_LEXEME_FORWARD_SLASH, "/", "/_"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_wildcards) +{ + EXPECT_LEX(RCL_LEXEME_WILD_ONE, "*", "*"); + EXPECT_LEX(RCL_LEXEME_WILD_ONE, "*", "*/"); + EXPECT_LEX(RCL_LEXEME_WILD_MULTI, "**", "**"); + EXPECT_LEX(RCL_LEXEME_WILD_MULTI, "**", "**/"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_colon) +{ + EXPECT_LEX(RCL_LEXEME_COLON, ":", ":"); + EXPECT_LEX(RCL_LEXEME_COLON, ":", ":r"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_separator) +{ + EXPECT_LEX(RCL_LEXEME_SEPARATOR, ":=", ":="); + EXPECT_LEX(RCL_LEXEME_SEPARATOR, ":=", ":=0"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_ns) +{ + // Has __ns + EXPECT_LEX(RCL_LEXEME_NS, "__ns", "__ns"); + EXPECT_LEX(RCL_LEXEME_NS, "__ns", "__nsssss"); + + // Things that are almost __ns + EXPECT_LEX(RCL_LEXEME_NONE, "__", "__"); + EXPECT_LEX(RCL_LEXEME_NONE, "__n", "__n"); + EXPECT_LEX(RCL_LEXEME_NONE, "__n!", "__n!"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_name) +{ + // Has __name + EXPECT_LEX(RCL_LEXEME_NODE, "__name", "__name"); + EXPECT_LEX(RCL_LEXEME_NODE, "__name", "__namessss"); + + // Things that are almost __name + EXPECT_LEX(RCL_LEXEME_NONE, "__na", "__na"); + EXPECT_LEX(RCL_LEXEME_NONE, "__naa", "__naa"); + EXPECT_LEX(RCL_LEXEME_NONE, "__nam", "__nam"); + EXPECT_LEX(RCL_LEXEME_NONE, "__nama", "__nama"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_node) +{ + // Has __node + EXPECT_LEX(RCL_LEXEME_NODE, "__node", "__node"); + EXPECT_LEX(RCL_LEXEME_NODE, "__node", "__nodessss"); + + // Things that are almost __node + EXPECT_LEX(RCL_LEXEME_NONE, "__", "__"); + EXPECT_LEX(RCL_LEXEME_NONE, "__n", "__n"); + EXPECT_LEX(RCL_LEXEME_NONE, "__ne", "__ne"); + EXPECT_LEX(RCL_LEXEME_NONE, "__no", "__no"); + EXPECT_LEX(RCL_LEXEME_NONE, "__noa", "__noa"); + EXPECT_LEX(RCL_LEXEME_NONE, "__nod", "__nod"); + EXPECT_LEX(RCL_LEXEME_NONE, "__noda", "__noda"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_tilde_slash) +{ + EXPECT_LEX(RCL_LEXEME_TILDE_SLASH, "~/", "~/"); + EXPECT_LEX(RCL_LEXEME_TILDE_SLASH, "~/", "~//"); + EXPECT_LEX(RCL_LEXEME_NONE, "~", "~"); + EXPECT_LEX(RCL_LEXEME_NONE, "~!", "~!"); +} + +TEST_F(CLASSNAME(TestLexerFixture, RMW_IMPLEMENTATION), test_eof) +{ + EXPECT_LEX(RCL_LEXEME_EOF, "", ""); +} diff --git a/test/rcl/test_lexer_lookahead.cpp b/test/rcl/test_lexer_lookahead.cpp new file mode 100644 index 0000000..d6dd181 --- /dev/null +++ b/test/rcl/test_lexer_lookahead.cpp @@ -0,0 +1,415 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "osrf_testing_tools_cpp/scope_exit.hpp" + +#include "rcl/error_handling.h" +#include "rcl/lexer_lookahead.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestLexerLookaheadFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + void SetUp() + { + } + + void TearDown() + { + } +}; + +#define SCOPE_LOOKAHEAD2(name, text) \ + { \ + name = rcl_get_zero_initialized_lexer_lookahead2(); \ + rcl_ret_t ret = rcl_lexer_lookahead2_init(&name, text, rcl_get_default_allocator()); \ + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + } \ + auto __scope_lookahead2_ ## name = osrf_testing_tools_cpp::make_scope_exit( \ + [&name]() { \ + rcl_ret_t ret = rcl_lexer_lookahead2_fini(&buffer); \ + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + }) + +TEST_F(CLASSNAME(TestLexerLookaheadFixture, RMW_IMPLEMENTATION), test_init_fini_twice) +{ + rcl_lexer_lookahead2_t buffer = rcl_get_zero_initialized_lexer_lookahead2(); + rcl_ret_t ret = rcl_lexer_lookahead2_init(&buffer, "foobar", rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_lexer_lookahead2_fini(&buffer); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_lexer_lookahead2_fini(&buffer); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestLexerLookaheadFixture, RMW_IMPLEMENTATION), test_init_not_zero_initialized) +{ + rcl_lexer_lookahead2_t buffer; + int not_zero = 1; + buffer.impl = reinterpret_cast(¬_zero); + rcl_ret_t ret = rcl_lexer_lookahead2_init(&buffer, "foobar", rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestLexerLookaheadFixture, RMW_IMPLEMENTATION), test_peek) +{ + rcl_ret_t ret; + rcl_lexer_lookahead2_t buffer; + SCOPE_LOOKAHEAD2(buffer, "foobar"); + + rcl_lexeme_t lexeme = RCL_LEXEME_NONE; + + ret = rcl_lexer_lookahead2_peek(&buffer, &lexeme); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(RCL_LEXEME_TOKEN, lexeme); + + // Test again to make sure peek isn't advancing the lexer + lexeme = RCL_LEXEME_NONE; + ret = rcl_lexer_lookahead2_peek(&buffer, &lexeme); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(RCL_LEXEME_TOKEN, lexeme); +} + +TEST_F(CLASSNAME(TestLexerLookaheadFixture, RMW_IMPLEMENTATION), test_peek2) +{ + rcl_ret_t ret; + rcl_lexer_lookahead2_t buffer; + SCOPE_LOOKAHEAD2(buffer, "foobar/"); + + rcl_lexeme_t lexeme1 = RCL_LEXEME_NONE; + rcl_lexeme_t lexeme2 = RCL_LEXEME_NONE; + + ret = rcl_lexer_lookahead2_peek2(&buffer, &lexeme1, &lexeme2); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_LEXEME_TOKEN, lexeme1); + EXPECT_EQ(RCL_LEXEME_FORWARD_SLASH, lexeme2); + + // Test again to make sure peek2 isn't advancing the lexer + lexeme1 = RCL_LEXEME_NONE; + lexeme2 = RCL_LEXEME_NONE; + ret = rcl_lexer_lookahead2_peek2(&buffer, &lexeme1, &lexeme2); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_LEXEME_TOKEN, lexeme1); + EXPECT_EQ(RCL_LEXEME_FORWARD_SLASH, lexeme2); +} + +TEST_F(CLASSNAME(TestLexerLookaheadFixture, RMW_IMPLEMENTATION), test_eof) +{ + rcl_ret_t ret; + rcl_lexer_lookahead2_t buffer; + SCOPE_LOOKAHEAD2(buffer, ""); + + { + rcl_lexeme_t lexeme = RCL_LEXEME_NONE; + ret = rcl_lexer_lookahead2_peek(&buffer, &lexeme); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(RCL_LEXEME_EOF, lexeme); + } + { + rcl_lexeme_t lexeme1 = RCL_LEXEME_NONE; + rcl_lexeme_t lexeme2 = RCL_LEXEME_NONE; + ret = rcl_lexer_lookahead2_peek2(&buffer, &lexeme1, &lexeme2); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_LEXEME_EOF, lexeme1); + EXPECT_EQ(RCL_LEXEME_EOF, lexeme2); + } + // Accepting keeps the lexer at EOF + { + EXPECT_EQ(RCL_RET_OK, rcl_lexer_lookahead2_accept(&buffer, NULL, NULL)); + rcl_lexeme_t lexeme = RCL_LEXEME_NONE; + ret = rcl_lexer_lookahead2_peek(&buffer, &lexeme); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(RCL_LEXEME_EOF, lexeme); + } +} + + +TEST_F(CLASSNAME(TestLexerLookaheadFixture, RMW_IMPLEMENTATION), test_accept) +{ + rcl_ret_t ret; + rcl_lexer_lookahead2_t buffer; + SCOPE_LOOKAHEAD2(buffer, "foobar/"); + + rcl_lexeme_t lexeme = RCL_LEXEME_NONE; + const char * lexeme_text; + size_t lexeme_text_length; + + // Peek token + ret = rcl_lexer_lookahead2_peek(&buffer, &lexeme); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_LEXEME_TOKEN, lexeme); + + // accept token + ret = rcl_lexer_lookahead2_accept(&buffer, &lexeme_text, &lexeme_text_length); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("foobar", std::string(lexeme_text, lexeme_text_length).c_str()); + + // peek forward slash + ret = rcl_lexer_lookahead2_peek(&buffer, &lexeme); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_LEXEME_FORWARD_SLASH, lexeme); + + // accept forward slash + ret = rcl_lexer_lookahead2_accept(&buffer, &lexeme_text, &lexeme_text_length); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/", std::string(lexeme_text, lexeme_text_length).c_str()); + + // peek eof + ret = rcl_lexer_lookahead2_peek(&buffer, &lexeme); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_LEXEME_EOF, lexeme); + + // accept eof + ret = rcl_lexer_lookahead2_accept(&buffer, &lexeme_text, &lexeme_text_length); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("", std::string(lexeme_text, lexeme_text_length).c_str()); + + // peek eof again + ret = rcl_lexer_lookahead2_peek(&buffer, &lexeme); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_LEXEME_EOF, lexeme); +} + +TEST_F(CLASSNAME(TestLexerLookaheadFixture, RMW_IMPLEMENTATION), test_accept_bad_arg) +{ + rcl_lexer_lookahead2_t buffer; + rcl_lexer_lookahead2_t buffer_not_ini = rcl_get_zero_initialized_lexer_lookahead2(); + SCOPE_LOOKAHEAD2(buffer, "foobar/"); + + rcl_lexeme_t lexeme = RCL_LEXEME_NONE; + const char * lexeme_text; + size_t lexeme_text_length = 0; + + // Can't accept without peek first + rcl_ret_t ret = rcl_lexer_lookahead2_accept(&buffer, &lexeme_text, &lexeme_text_length); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Expected usage + ret = rcl_lexer_lookahead2_peek(&buffer, &lexeme); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_LEXEME_TOKEN, lexeme); + + // Invalid nullptr parameter + ret = rcl_lexer_lookahead2_accept(nullptr, &lexeme_text, &lexeme_text_length); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Invalid not ini parameter + ret = rcl_lexer_lookahead2_accept(&buffer_not_ini, &lexeme_text, &lexeme_text_length); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Invalid nullptr as lexeme_text_length + ret = rcl_lexer_lookahead2_accept(&buffer, &lexeme_text, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Invalid nullptr as lexeme_text + ret = rcl_lexer_lookahead2_accept(&buffer, nullptr, &lexeme_text_length); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestLexerLookaheadFixture, RMW_IMPLEMENTATION), test_expect) +{ + rcl_ret_t ret; + rcl_lexer_lookahead2_t buffer; + SCOPE_LOOKAHEAD2(buffer, "node_name:__node:=new_1"); + const char * lexeme_text; + size_t lexeme_text_length; + + ret = rcl_lexer_lookahead2_expect(&buffer, RCL_LEXEME_TOKEN, &lexeme_text, &lexeme_text_length); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("node_name", std::string(lexeme_text, lexeme_text_length).c_str()); + + ret = rcl_lexer_lookahead2_expect( + &buffer, RCL_LEXEME_FORWARD_SLASH, &lexeme_text, &lexeme_text_length); + EXPECT_EQ(RCL_RET_WRONG_LEXEME, ret) << rcl_get_error_string().str; +} + +#define EXPECT_LOOKAHEAD(expected_lexeme, expected_text, buffer) \ + do { \ + const char * lexeme_text; \ + size_t lexeme_text_length; \ + rcl_lexeme_t lexeme; \ + ret = rcl_lexer_lookahead2_peek(&buffer, &lexeme); \ + EXPECT_EQ(expected_lexeme, lexeme); \ + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + ret = rcl_lexer_lookahead2_accept(&buffer, &lexeme_text, &lexeme_text_length); \ + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + EXPECT_STREQ(expected_text, std::string(lexeme_text, lexeme_text_length).c_str()); \ + } while (false) + +TEST_F(CLASSNAME(TestLexerLookaheadFixture, RMW_IMPLEMENTATION), test_lex_long_string) +{ + rcl_ret_t ret; + rcl_lexer_lookahead2_t buffer; + SCOPE_LOOKAHEAD2(buffer, ":\\1rostopic://\\2rosservice://~/\\8:=**:*foobar"); + + EXPECT_LOOKAHEAD(RCL_LEXEME_COLON, ":", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_BR1, "\\1", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_URL_TOPIC, "rostopic://", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_BR2, "\\2", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_URL_SERVICE, "rosservice://", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TILDE_SLASH, "~/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_BR8, "\\8", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_SEPARATOR, ":=", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_WILD_MULTI, "**", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_COLON, ":", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_WILD_ONE, "*", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "foobar", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_EOF, "", buffer); +} + +TEST_F(CLASSNAME(TestLexerLookaheadFixture, RMW_IMPLEMENTATION), test_lex_remap_rules) +{ + rcl_ret_t ret; + rcl_lexer_lookahead2_t buffer; + { + SCOPE_LOOKAHEAD2(buffer, "foo:=bar"); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "foo", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_SEPARATOR, ":=", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "bar", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_EOF, "", buffer); + } + { + SCOPE_LOOKAHEAD2(buffer, "/foo/bar:=fiz/buzz"); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "foo", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "bar", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_SEPARATOR, ":=", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "fiz", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "buzz", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_EOF, "", buffer); + } + { + // Nodename prefix + SCOPE_LOOKAHEAD2(buffer, "nodename:~/foo:=foo"); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "nodename", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_COLON, ":", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TILDE_SLASH, "~/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "foo", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_SEPARATOR, ":=", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "foo", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_EOF, "", buffer); + } + { + // Partial namespace replacement + SCOPE_LOOKAHEAD2(buffer, "/foo/**:=/fizz/\\1"); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "foo", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_WILD_MULTI, "**", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_SEPARATOR, ":=", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "fizz", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_BR1, "\\1", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_EOF, "", buffer); + } + { + // Full namespace replacement + SCOPE_LOOKAHEAD2(buffer, "/foo/bar/*:=/bar/foo/\\1"); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "foo", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "bar", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_WILD_ONE, "*", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_SEPARATOR, ":=", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "bar", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "foo", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_BR1, "\\1", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_EOF, "", buffer); + } + { + // Change a base name + SCOPE_LOOKAHEAD2(buffer, "**/foo:=\\1/bar"); + EXPECT_LOOKAHEAD(RCL_LEXEME_WILD_MULTI, "**", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "foo", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_SEPARATOR, ":=", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_BR1, "\\1", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "bar", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_EOF, "", buffer); + } + { + // Change namespace + SCOPE_LOOKAHEAD2(buffer, "__ns:=/new/namespace"); + EXPECT_LOOKAHEAD(RCL_LEXEME_NS, "__ns", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_SEPARATOR, ":=", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "new", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "namespace", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_EOF, "", buffer); + } + { + // Change node name + SCOPE_LOOKAHEAD2(buffer, "__node:=left_camera_driver"); + EXPECT_LOOKAHEAD(RCL_LEXEME_NODE, "__node", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_SEPARATOR, ":=", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "left_camera_driver", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_EOF, "", buffer); + } + { + // Topic only remap + SCOPE_LOOKAHEAD2(buffer, "rostopic://foo/bar:=bar/foo"); + EXPECT_LOOKAHEAD(RCL_LEXEME_URL_TOPIC, "rostopic://", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "foo", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "bar", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_SEPARATOR, ":=", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "bar", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "foo", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_EOF, "", buffer); + } + { + // Service only remap + SCOPE_LOOKAHEAD2(buffer, "rosservice:///foo/bar:=/bar/foo"); + EXPECT_LOOKAHEAD(RCL_LEXEME_URL_SERVICE, "rosservice://", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "foo", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "bar", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_SEPARATOR, ":=", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "bar", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_FORWARD_SLASH, "/", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_TOKEN, "foo", buffer); + EXPECT_LOOKAHEAD(RCL_LEXEME_EOF, "", buffer); + } +} diff --git a/test/rcl/test_localhost.cpp b/test/rcl/test_localhost.cpp new file mode 100644 index 0000000..e781b75 --- /dev/null +++ b/test/rcl/test_localhost.cpp @@ -0,0 +1,41 @@ +// 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 "rcl/rcl.h" +#include "rcl/localhost.h" +#include "rmw/localhost.h" +#include "rcutils/env.h" + +TEST(TestLocalhost, test_get_localhost_only) { + ASSERT_TRUE(rcutils_set_env("ROS_LOCALHOST_ONLY", "0")); + rmw_localhost_only_t localhost_var; + EXPECT_EQ(RCL_RET_OK, rcl_get_localhost_only(&localhost_var)); + EXPECT_EQ(RMW_LOCALHOST_ONLY_DISABLED, localhost_var); + + ASSERT_TRUE(rcutils_set_env("ROS_LOCALHOST_ONLY", "1")); + EXPECT_EQ(RCL_RET_OK, rcl_get_localhost_only(&localhost_var)); + EXPECT_EQ(RMW_LOCALHOST_ONLY_ENABLED, localhost_var); + + ASSERT_TRUE(rcutils_set_env("ROS_LOCALHOST_ONLY", "2")); + EXPECT_EQ(RCL_RET_OK, rcl_get_localhost_only(&localhost_var)); + EXPECT_EQ(RMW_LOCALHOST_ONLY_DISABLED, localhost_var); + + ASSERT_TRUE(rcutils_set_env("ROS_LOCALHOST_ONLY", "Unexpected")); + EXPECT_EQ(RCL_RET_OK, rcl_get_localhost_only(&localhost_var)); + EXPECT_EQ(RMW_LOCALHOST_ONLY_DISABLED, localhost_var); + + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_get_localhost_only(nullptr)); +} diff --git a/test/rcl/test_log_level.cpp b/test/rcl/test_log_level.cpp new file mode 100644 index 0000000..a53a3ea --- /dev/null +++ b/test/rcl/test_log_level.cpp @@ -0,0 +1,408 @@ +// 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 "rcl/allocator.h" +#include "rcl/rcl.h" +#include "rcl/log_level.h" +#include "rcl/error_handling.h" +#include "rcutils/logging.h" + +#include "./arg_macros.hpp" +#include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" + +int setup_and_parse_log_level_args(const char * log_level_string) +{ + rcl_arguments_t local_arguments = rcl_get_zero_initialized_arguments(); + const char * local_argv[] = {"process_name", "--ros-args", "--log-level", log_level_string}; + unsigned int local_argc = (sizeof(local_argv) / sizeof(const char *)); + return rcl_parse_arguments( + local_argc, local_argv, rcl_get_default_allocator(), &local_arguments); +} + +TEST(TestLogLevel, error_log_level) { + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args(":=debug")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("debug,")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:=debug,")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:=debug,,")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:=")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:=,")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args(":")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args(":=")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl=")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl=debug")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:=:=")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl=debug,")); + rcl_reset_error(); + ASSERT_EQ(RCL_RET_INVALID_ROS_ARGS, setup_and_parse_log_level_args("rcl:,")); + rcl_reset_error(); +} + +#define GET_LOG_LEVEL_FROM_ARGUMENTS(log_levels, ...) \ + { \ + rcl_ret_t ret; \ + rcl_arguments_t local_arguments = rcl_get_zero_initialized_arguments(); \ + const char * local_argv[] = {__VA_ARGS__}; \ + unsigned int local_argc = (sizeof(local_argv) / sizeof(const char *)); \ + ret = rcl_parse_arguments( \ + local_argc, local_argv, rcl_get_default_allocator(), &local_arguments); \ + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + ret = rcl_arguments_get_log_levels(&local_arguments, &log_levels); \ + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( \ + { \ + ASSERT_EQ(RCL_RET_OK, rcl_arguments_fini(&local_arguments)); \ + }); \ + } + +TEST(TestLogLevel, no_log_level) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS(log_levels, "process_name"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, log_levels.default_logger_level); + EXPECT_EQ(0ul, log_levels.num_logger_settings); +} + +TEST(TestLogLevel, default_log_level) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "debug"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.default_logger_level); + EXPECT_EQ(0ul, log_levels.num_logger_settings); +} + +TEST(TestLogLevel, logger_log_level_debug) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "rcl:=debug"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.logger_settings[0].level); +} + +TEST(TestLogLevel, logger_log_level_info) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "rcl:=info"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, log_levels.logger_settings[0].level); +} + +TEST(TestLogLevel, multiple_log_level_with_default_at_front) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "debug", "--log-level", "rcl:=debug"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.logger_settings[0].level); +} + +TEST(TestLogLevel, multiple_log_level_with_default_at_back) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "rcl:=debug", "--log-level", "debug"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.logger_settings[0].level); +} + +TEST(TestLogLevel, multiple_log_level_rightmost_prevail) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "debug", "--log-level", "info", + "--log-level", "rcl:=debug", "--log-level", "rcl:=info"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, log_levels.logger_settings[0].level); +} + +TEST(TestLogLevel, multiple_log_level_names) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "debug", "--log-level", "rcl:=debug", + "--log-level", "test:=info"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.default_logger_level); + EXPECT_EQ(2ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.logger_settings[0].level); + EXPECT_STREQ("test", log_levels.logger_settings[1].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, log_levels.logger_settings[1].level); +} + +TEST(TestLogLevel, log_level_dot_logger_name) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "test.abc:=info"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("test.abc", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, log_levels.logger_settings[0].level); +} + +TEST(TestLogLevel, log_level_init_fini) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + + // Test zero size ini/fini + const size_t zero_count = 0; + EXPECT_EQ( + RCL_RET_OK, + rcl_log_levels_init(&log_levels, &allocator, zero_count)); + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + + const size_t capacity_count = 1; + EXPECT_EQ( + RCL_RET_OK, + rcl_log_levels_init(&log_levels, &allocator, capacity_count)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_log_levels_init(nullptr, &allocator, capacity_count)); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_log_levels_init(&log_levels, nullptr, capacity_count)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_log_levels_init(&log_levels, &allocator, capacity_count)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + rcl_allocator_t bad_allocator = get_failing_allocator(); + rcl_log_levels_t empty_log_levels = rcl_get_zero_initialized_log_levels(); + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_log_levels_init(&empty_log_levels, &bad_allocator, capacity_count)); + + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_log_levels_fini(nullptr)); +} + +TEST(TestLogLevel, logger_log_level_copy) { + // Init to debug level to test before copy + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + GET_LOG_LEVEL_FROM_ARGUMENTS( + log_levels, "process_name", "--ros-args", + "--log-level", "rcl:=debug"); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.logger_settings[0].level); + + // Expected usage + rcl_log_levels_t copied_log_levels = rcl_get_zero_initialized_log_levels(); + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_copy(&log_levels, &copied_log_levels)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&copied_log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, copied_log_levels.default_logger_level); + EXPECT_EQ(log_levels.default_logger_level, copied_log_levels.default_logger_level); + EXPECT_EQ(1ul, copied_log_levels.num_logger_settings); + EXPECT_EQ(log_levels.num_logger_settings, copied_log_levels.num_logger_settings); + EXPECT_STREQ("rcl", copied_log_levels.logger_settings[0].name); + EXPECT_STREQ(log_levels.logger_settings[0].name, copied_log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, copied_log_levels.logger_settings[0].level); + EXPECT_EQ(log_levels.logger_settings[0].level, copied_log_levels.logger_settings[0].level); + + // Bad usage + rcl_log_levels_t empty_log_levels = rcl_get_zero_initialized_log_levels(); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_log_levels_copy(nullptr, &empty_log_levels)); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_log_levels_copy(&log_levels, nullptr)); + // Already copied + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_log_levels_copy(&log_levels, &copied_log_levels)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // null alloc + rcl_allocator_t saved_allocator = log_levels.allocator; + log_levels.allocator = {NULL, NULL, NULL, NULL, NULL}; + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_log_levels_copy(&log_levels, &empty_log_levels)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // bad allocation + rcl_allocator_t bad_allocator = get_failing_allocator(); + log_levels.allocator = bad_allocator; + EXPECT_EQ(RCL_RET_BAD_ALLOC, rcl_log_levels_copy(&log_levels, &empty_log_levels)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + log_levels.allocator = saved_allocator; +} + +TEST(TestLogLevel, test_add_logger_setting) { + rcl_log_levels_t log_levels = rcl_get_zero_initialized_log_levels(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + size_t logger_count = 2u; + EXPECT_EQ( + RCL_RET_OK, rcl_log_levels_init(&log_levels, &allocator, logger_count)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_log_levels_fini(&log_levels)); + }); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, log_levels.default_logger_level); + EXPECT_EQ(0ul, log_levels.num_logger_settings); + + // Invalid arguments + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_log_levels_add_logger_setting(nullptr, "rcl", RCUTILS_LOG_SEVERITY_DEBUG)); + + rcl_log_levels_t not_ini_log_levels = rcl_get_zero_initialized_log_levels(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_log_levels_add_logger_setting(¬_ini_log_levels, "rcl", RCUTILS_LOG_SEVERITY_DEBUG)); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_log_levels_add_logger_setting(&log_levels, nullptr, RCUTILS_LOG_SEVERITY_DEBUG)); + + rcl_allocator_t saved_allocator = log_levels.allocator; + log_levels.allocator = {NULL, NULL, NULL, NULL, NULL}; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_log_levels_add_logger_setting(&log_levels, "rcl", RCUTILS_LOG_SEVERITY_DEBUG)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + rcl_allocator_t bad_allocator = get_failing_allocator(); + log_levels.allocator = bad_allocator; + EXPECT_EQ( + RCL_RET_BAD_ALLOC, + rcl_log_levels_add_logger_setting(&log_levels, "rcl", RCUTILS_LOG_SEVERITY_DEBUG)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + log_levels.allocator = saved_allocator; + + // Expected usage + EXPECT_EQ( + RCL_RET_OK, rcl_log_levels_add_logger_setting(&log_levels, "rcl", RCUTILS_LOG_SEVERITY_DEBUG)); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, log_levels.default_logger_level); + EXPECT_EQ(1ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.logger_settings[0].level); + + EXPECT_EQ( + RCL_RET_OK, + rcl_log_levels_add_logger_setting(&log_levels, "rcutils", RCUTILS_LOG_SEVERITY_INFO)); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, log_levels.default_logger_level); + EXPECT_EQ(2ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, log_levels.logger_settings[0].level); + EXPECT_STREQ("rcutils", log_levels.logger_settings[1].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, log_levels.logger_settings[1].level); + + // Can't add more than logger_count + EXPECT_EQ( + RCL_RET_ERROR, + rcl_log_levels_add_logger_setting(&log_levels, "rmw", RCUTILS_LOG_SEVERITY_DEBUG)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + EXPECT_EQ(2ul, log_levels.num_logger_settings); + + // Replacing saved logger + EXPECT_EQ( + RCL_RET_OK, + rcl_log_levels_add_logger_setting(&log_levels, "rcl", RCUTILS_LOG_SEVERITY_INFO)); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_UNSET, log_levels.default_logger_level); + EXPECT_EQ(2ul, log_levels.num_logger_settings); + EXPECT_STREQ("rcl", log_levels.logger_settings[0].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, log_levels.logger_settings[0].level); + EXPECT_STREQ("rcutils", log_levels.logger_settings[1].name); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, log_levels.logger_settings[1].level); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + // We need to do rcutils_logging_shutdown() since the tests above may + // have initialized logging. + if (rcutils_logging_shutdown() != RCUTILS_RET_OK) { + fprintf(stderr, "Failed shutting down rcutils logging\n"); + } + return ret; +} diff --git a/test/rcl/test_logging.cpp b/test/rcl/test_logging.cpp new file mode 100644 index 0000000..1ce2eda --- /dev/null +++ b/test/rcl/test_logging.cpp @@ -0,0 +1,302 @@ +// 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 "osrf_testing_tools_cpp/scope_exit.hpp" + +#include "rcl/error_handling.h" +#include "rcl/logging.h" +#include "rcl/rcl.h" +#include "rcl/subscription.h" +#include "rcl_interfaces/msg/log.h" +#include "rcl_logging_interface/rcl_logging_interface.h" + +#include "rcutils/logging_macros.h" + +#include "../mocking_utils/patch.hpp" + +// Define dummy comparison operators for rcutils_allocator_t type +// to use with the Mimick mocking library +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) + +TEST(TestLogging, test_configure_with_bad_arguments) { + rcl_allocator_t default_allocator = rcl_get_default_allocator(); + const char * argv[] = {"test_logging"}; + const int argc = sizeof(argv) / sizeof(argv[0]); + rcl_arguments_t global_arguments = rcl_get_zero_initialized_arguments(); + ASSERT_EQ(RCL_RET_OK, rcl_parse_arguments(argc, argv, default_allocator, &global_arguments)) << + rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&global_arguments)) << rcl_get_error_string().str; + }); + + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_logging_configure(nullptr, &default_allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_logging_configure(&global_arguments, nullptr)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + rcl_allocator_t zero_initialized_allocator = rcutils_get_zero_initialized_allocator(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_logging_configure(&global_arguments, &zero_initialized_allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_logging_configure_with_output_handler( + nullptr, &default_allocator, rcl_logging_multiple_output_handler)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_logging_configure_with_output_handler( + &global_arguments, nullptr, rcl_logging_multiple_output_handler)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_logging_configure_with_output_handler( + &global_arguments, &default_allocator, nullptr)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_logging_configure_with_output_handler( + &global_arguments, &zero_initialized_allocator, rcl_logging_multiple_output_handler)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +TEST(TestLogging, test_logging_rosout_enabled) { + { + const char * argv[] = { + "test_logging", RCL_ROS_ARGS_FLAG, "--enable-" RCL_LOG_ROSOUT_FLAG_SUFFIX}; + const int argc = sizeof(argv) / sizeof(argv[0]); + rcl_allocator_t default_allocator = rcl_get_default_allocator(); + rcl_arguments_t global_arguments = rcl_get_zero_initialized_arguments(); + ASSERT_EQ(RCL_RET_OK, rcl_parse_arguments(argc, argv, default_allocator, &global_arguments)) << + rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&global_arguments)) << rcl_get_error_string().str; + }); + ASSERT_EQ(RCL_RET_OK, rcl_logging_configure(&global_arguments, &default_allocator)) << + rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_logging_fini()) << rcl_get_error_string().str; + }); + + ASSERT_TRUE(rcl_logging_rosout_enabled()); + } + + { + const char * argv[] = { + "test_logging", RCL_ROS_ARGS_FLAG, "--disable-" RCL_LOG_ROSOUT_FLAG_SUFFIX}; + const int argc = sizeof(argv) / sizeof(argv[0]); + rcl_allocator_t default_allocator = rcl_get_default_allocator(); + rcl_arguments_t global_arguments = rcl_get_zero_initialized_arguments(); + ASSERT_EQ(RCL_RET_OK, rcl_parse_arguments(argc, argv, default_allocator, &global_arguments)) << + rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&global_arguments)) << rcl_get_error_string().str; + }); + ASSERT_EQ(RCL_RET_OK, rcl_logging_configure(&global_arguments, &default_allocator)) << + rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_logging_fini()) << rcl_get_error_string().str; + }); + + ASSERT_FALSE(rcl_logging_rosout_enabled()); + } +} + +TEST(TestLogging, test_failing_external_logging_configure) { + const char * argv[] = { + "test_logging", RCL_ROS_ARGS_FLAG, + "--enable-" RCL_LOG_EXT_LIB_FLAG_SUFFIX}; + const int argc = sizeof(argv) / sizeof(argv[0]); + rcl_allocator_t default_allocator = rcl_get_default_allocator(); + rcl_arguments_t global_arguments = rcl_get_zero_initialized_arguments(); + ASSERT_EQ(RCL_RET_OK, rcl_parse_arguments(argc, argv, default_allocator, &global_arguments)) << + rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&global_arguments)) << rcl_get_error_string().str; + }); + + { + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rcl_logging_external_initialize, "some error", RCL_LOGGING_RET_ERROR); + EXPECT_EQ(RCL_LOGGING_RET_ERROR, rcl_logging_configure(&global_arguments, &default_allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_OK, rcl_logging_fini()) << rcl_get_error_string().str; + } + + { + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rcl_logging_external_set_logger_level, "some error", RCL_LOGGING_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_logging_configure(&global_arguments, &default_allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_OK, rcl_logging_fini()) << rcl_get_error_string().str; + } +} + +TEST(TestLogging, test_failing_logger_level_configure) { + const char * argv[] = { + "test_logging", RCL_ROS_ARGS_FLAG, + RCL_LOG_LEVEL_FLAG, ROS_PACKAGE_NAME ":=info"}; + const int argc = sizeof(argv) / sizeof(argv[0]); + rcl_allocator_t default_allocator = rcl_get_default_allocator(); + rcl_arguments_t global_arguments = rcl_get_zero_initialized_arguments(); + ASSERT_EQ(RCL_RET_OK, rcl_parse_arguments(argc, argv, default_allocator, &global_arguments)) << + rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&global_arguments)) << rcl_get_error_string().str; + }); + + { + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rcutils_logging_set_logger_level, "failed to allocate", RCUTILS_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_logging_configure(&global_arguments, &default_allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_OK, rcl_logging_fini()) << rcl_get_error_string().str; + } +} + +TEST(TestLogging, test_failing_external_logging) { + const char * argv[] = { + "test_logging", RCL_ROS_ARGS_FLAG, + "--disable-" RCL_LOG_STDOUT_FLAG_SUFFIX, + "--enable-" RCL_LOG_EXT_LIB_FLAG_SUFFIX, + RCL_LOG_LEVEL_FLAG, ROS_PACKAGE_NAME ":=DEBUG" + }; + const int argc = sizeof(argv) / sizeof(argv[0]); + rcl_allocator_t default_allocator = rcl_get_default_allocator(); + rcl_arguments_t global_arguments = rcl_get_zero_initialized_arguments(); + ASSERT_EQ(RCL_RET_OK, rcl_parse_arguments(argc, argv, default_allocator, &global_arguments)) << + rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&global_arguments)) << rcl_get_error_string().str; + }); + ASSERT_EQ(RCL_RET_OK, rcl_logging_configure(&global_arguments, &default_allocator)) << + rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_logging_fini()) << rcl_get_error_string().str; + }); + + bool log_seen = false; + int severity_seen = RCUTILS_LOG_SEVERITY_UNSET; + std::string logger_name_seen; + std::string log_message_seen; + auto log_mock = mocking_utils::patch( + "lib:rcl", rcl_logging_external_log, + [&](int severity, const char * name, const char * message) { + severity_seen = severity; + logger_name_seen = name; + log_message_seen = message; + log_seen = true; + }); + + constexpr char log_message[] = "Test message"; + RCUTILS_LOG_DEBUG_NAMED(ROS_PACKAGE_NAME, log_message); + EXPECT_TRUE(log_seen); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_DEBUG, severity_seen); + EXPECT_EQ(ROS_PACKAGE_NAME, logger_name_seen); + EXPECT_TRUE(log_message_seen.find(log_message) != std::string::npos) << + "Expected '" << log_message << "' within '" << log_message_seen << "'"; + + std::stringstream stderr_sstream; + auto fwrite_mock = mocking_utils::patch( + "lib:rcl", fwrite, + [&](const void * ptr, size_t size, size_t count, FILE * stream) + { + if (sizeof(char) == size && stderr == stream) { + stderr_sstream << std::string(reinterpret_cast(ptr), count); + } + return count; + }); + + constexpr char stderr_message[] = "internal error"; +#ifdef MOCKING_UTILS_SUPPORT_VA_LIST + { + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rcutils_char_array_vsprintf, + stderr_message, RCUTILS_RET_ERROR); + + log_seen = false; + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, log_message); + EXPECT_TRUE(log_seen); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_INFO, severity_seen); + EXPECT_EQ(ROS_PACKAGE_NAME, logger_name_seen); + EXPECT_TRUE(stderr_sstream.str().find(stderr_message) != std::string::npos) << + "Expected '" << stderr_message << "' within '" << stderr_sstream.str() << "'"; + stderr_sstream.str(""); + } +#endif + + { + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rcutils_logging_format_message, + stderr_message, RCUTILS_RET_ERROR); + + log_seen = false; + RCUTILS_LOG_WARN_NAMED(ROS_PACKAGE_NAME, log_message); + EXPECT_TRUE(log_seen); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_WARN, severity_seen); + EXPECT_EQ(ROS_PACKAGE_NAME, logger_name_seen); + EXPECT_TRUE(stderr_sstream.str().find(stderr_message) != std::string::npos) << + "Expected '" << stderr_message << "' within '" << stderr_sstream.str() << "'"; + stderr_sstream.str(""); + } + + { + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rcutils_char_array_fini, + stderr_message, RCUTILS_RET_ERROR); + + log_seen = false; + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, log_message); + EXPECT_TRUE(log_seen); + EXPECT_EQ(RCUTILS_LOG_SEVERITY_ERROR, severity_seen); + EXPECT_EQ(ROS_PACKAGE_NAME, logger_name_seen); + EXPECT_TRUE(log_message_seen.find(log_message) != std::string::npos) << + "Expected '" << log_message << "' within '" << log_message_seen << "'"; + EXPECT_TRUE(stderr_sstream.str().find(stderr_message) != std::string::npos) << + "Expected '" << stderr_message << "' within '" << stderr_sstream.str() << "'"; + stderr_sstream.str(""); + } +} diff --git a/test/rcl/test_logging_rosout.cpp b/test/rcl/test_logging_rosout.cpp new file mode 100644 index 0000000..224d180 --- /dev/null +++ b/test/rcl/test_logging_rosout.cpp @@ -0,0 +1,317 @@ +// Copyright 2019 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rcl/logging.h" +#include "rcl/rcl.h" +#include "rcl/subscription.h" +#include "rcl_interfaces/msg/log.h" +#include "rcutils/logging_macros.h" + +#include "rcl/logging_rosout.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +#define EXPAND(x) x +#define TEST_FIXTURE_P_RMW(test_fixture_name) CLASSNAME( \ + test_fixture_name, RMW_IMPLEMENTATION) +#define APPLY(macro, ...) EXPAND(macro(__VA_ARGS__)) +#define TEST_P_RMW(test_case_name, test_name) \ + APPLY( \ + TEST_P, CLASSNAME(test_case_name, RMW_IMPLEMENTATION), test_name) +#define INSTANTIATE_TEST_SUITE_P_RMW(instance_name, test_case_name, ...) \ + EXPAND( \ + APPLY( \ + INSTANTIATE_TEST_SUITE_P, instance_name, \ + CLASSNAME(test_case_name, RMW_IMPLEMENTATION), __VA_ARGS__)) + +struct TestParameters +{ + int argc; + const char ** argv; + bool enable_node_option_rosout; + bool expected_success; + std::string description; +}; + +// for ::testing::PrintToStringParamName() to show the exact test case name +std::ostream & operator<<( + std::ostream & out, + const TestParameters & params) +{ + out << params.description; + return out; +} + +class CLASSNAME (TestLogRosoutFixtureNotParam, RMW_IMPLEMENTATION) : public ::testing::Test {}; + +class TEST_FIXTURE_P_RMW (TestLoggingRosoutFixture) + : public ::testing::TestWithParam +{ +public: + void SetUp() + { + auto param = GetParam(); + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + + ret = rcl_init(param.argc, param.argv, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_EQ( + RCL_RET_OK, + rcl_logging_configure(&this->context_ptr->global_arguments, &allocator) + ) << rcl_get_error_string().str; + + // create node + rcl_node_options_t node_options = rcl_node_get_default_options(); + if (!param.enable_node_option_rosout) { + node_options.enable_rosout = param.enable_node_option_rosout; + } + const char * name = "test_rcl_node_logging_rosout"; + const char * namespace_ = "/ns"; + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + ret = rcl_node_init(this->node_ptr, name, namespace_, this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // create rosout subscription + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(rcl_interfaces, msg, Log); + const char * topic = "/rosout"; + this->subscription_ptr = new rcl_subscription_t; + *this->subscription_ptr = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init( + this->subscription_ptr, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret = rcl_subscription_fini(this->subscription_ptr, this->node_ptr); + delete this->subscription_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_logging_fini()) << rcl_get_error_string().str; + } + +protected: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + rcl_subscription_t * subscription_ptr; +}; + +void +check_if_rosout_subscription_gets_a_message( + const char * logger_name, + rcl_subscription_t * subscription, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms, + bool & success) +{ + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, context, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + size_t iteration = 0; + do { + RCUTILS_LOG_INFO_NAMED(logger_name, "SOMETHING"); + ++iteration; + ret = rcl_wait_set_clear(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_subscription(&wait_set, subscription, NULL); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); + if (ret == RCL_RET_TIMEOUT) { + continue; + } + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + for (size_t i = 0; i < wait_set.size_of_subscriptions; ++i) { + if (wait_set.subscriptions[i] && wait_set.subscriptions[i] == subscription) { + success = true; + return; + } + } + } while (iteration < max_tries); + success = false; +} + +/* Testing the subscriber of topic 'rosout' whether to get event from logging or not. + */ +TEST_P_RMW(TestLoggingRosoutFixture, test_logging_rosout) { + bool success = false; + check_if_rosout_subscription_gets_a_message( + rcl_node_get_logger_name(this->node_ptr), this->subscription_ptr, + this->context_ptr, 30, 100, success); + ASSERT_EQ(success, GetParam().expected_success); +} + +// +// create set of input and expected values +// +std::vector +get_parameters() +{ + std::vector parameters; + static int s_argc = 2; + static const char * s_argv_enable_rosout[] = {"--ros-args", "--enable-rosout-logs"}; + static const char * s_argv_disable_rosout[] = {"--ros-args", "--disable-rosout-logs"}; + + /* + * Test with enable(implicit) global rosout logs and enable node option of rosout. + */ + parameters.push_back( + { + 0, + nullptr, + true, + true, + "test_enable_implicit_global_rosout_enable_node_option" + }); + /* + * Test with enable(implicit) global rosout logs and disable node option of rosout. + */ + parameters.push_back( + { + 0, + nullptr, + false, + false, + "test_enable_implicit_global_rosout_disable_node_option" + }); + /* + * Test with enable(explicit) global rosout logs and enable node option of rosout. + */ + parameters.push_back( + { + s_argc, + s_argv_enable_rosout, + true, + true, + "test_enable_explicit_global_rosout_enable_node_option" + }); + /* + * Test with enable(explicit) global rosout logs and disable node option of rosout. + */ + parameters.push_back( + { + s_argc, + s_argv_enable_rosout, + false, + false, + "test_enable_explicit_global_rosout_disable_node_option" + }); + /* + * Test with disable global rosout logs and enable node option of rosout. + */ + parameters.push_back( + { + s_argc, + s_argv_disable_rosout, + true, + false, + "test_disable_global_rosout_enable_node_option" + }); + /* + * Test with disable global rosout logs and disable node option of rosout. + */ + parameters.push_back( + { + s_argc, + s_argv_disable_rosout, + false, + false, + "test_disable_global_rosout_disable_node_option" + }); + + return parameters; +} + +INSTANTIATE_TEST_SUITE_P_RMW( + TestLoggingRosoutWithDifferentSettings, + TestLoggingRosoutFixture, + ::testing::ValuesIn(get_parameters()), + ::testing::PrintToStringParamName()); + +/* Testing twice init logging_rosout + */ +TEST_F( + CLASSNAME(TestLogRosoutFixtureNotParam, RMW_IMPLEMENTATION), test_twice_init_logging_rosout) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + EXPECT_EQ(RCL_RET_OK, rcl_logging_rosout_init(&allocator)); + + // Init twice returns RCL_RET_OK + EXPECT_EQ(RCL_RET_OK, rcl_logging_rosout_init(&allocator)); + + EXPECT_EQ(RCL_RET_OK, rcl_logging_rosout_fini()); +} + +/* Bad params + */ +TEST_F( + CLASSNAME( + TestLogRosoutFixtureNotParam, RMW_IMPLEMENTATION), + test_bad_params_init_fini_node_publisher) +{ + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_node_t not_init_node = rcl_get_zero_initialized_node(); + EXPECT_EQ(RCL_RET_OK, rcl_logging_rosout_init(&allocator)); + + EXPECT_EQ(RCL_RET_NODE_INVALID, rcl_logging_rosout_init_publisher_for_node(nullptr)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_logging_rosout_init_publisher_for_node(¬_init_node)); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_NODE_INVALID, rcl_logging_rosout_fini_publisher_for_node(nullptr)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_logging_rosout_fini_publisher_for_node(¬_init_node)); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_OK, rcl_logging_rosout_fini()); +} diff --git a/test/rcl/test_network_flow_endpoints.cpp b/test/rcl/test_network_flow_endpoints.cpp new file mode 100644 index 0000000..0b97fb5 --- /dev/null +++ b/test/rcl/test_network_flow_endpoints.cpp @@ -0,0 +1,412 @@ +// Copyright 2020 Ericsson AB +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT 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 "mimick/mimick.h" +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rcl/network_flow_endpoints.h" +#include "rcl/publisher.h" +#include "rcl/rcl.h" +#include "rcl/subscription.h" +#include "test_msgs/msg/basic_types.h" + +#include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestNetworkFlowEndpointsNode, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + constexpr char name[] = "test_network_flow_endpoints_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +class CLASSNAME (TestPublisherNetworkFlowEndpoints, RMW_IMPLEMENTATION) + : public CLASSNAME(TestNetworkFlowEndpointsNode, RMW_IMPLEMENTATION) +{ +public: + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + const char * topic_1 = "chatter"; + const char * topic_2 = "mutter"; + const char * topic_3 = "sing"; + + rcl_publisher_t publisher_1; + rcl_publisher_t publisher_2; + rcl_publisher_t publisher_3; + + rcl_publisher_options_t publisher_1_options; + rcl_publisher_options_t publisher_2_options; + rcl_publisher_options_t publisher_3_options; + + void SetUp() override + { + CLASSNAME(TestNetworkFlowEndpointsNode, RMW_IMPLEMENTATION) ::SetUp(); + + publisher_1 = rcl_get_zero_initialized_publisher(); + publisher_1_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = rcl_publisher_init( + &publisher_1, this->node_ptr, ts, topic_1, &publisher_1_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + publisher_2 = rcl_get_zero_initialized_publisher(); + publisher_2_options = rcl_publisher_get_default_options(); + publisher_2_options.rmw_publisher_options.require_unique_network_flow_endpoints = + RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED; + ret = rcl_publisher_init( + &publisher_2, this->node_ptr, ts, topic_2, + &publisher_2_options); + ASSERT_TRUE( + ret == RCL_RET_OK || ret == RCL_RET_UNSUPPORTED || + ret == RCL_RET_ERROR) << rcl_get_error_string().str; + rcl_reset_error(); + + publisher_3 = rcl_get_zero_initialized_publisher(); + publisher_3_options = rcl_publisher_get_default_options(); + publisher_3_options.rmw_publisher_options.require_unique_network_flow_endpoints = + RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED; + ret = rcl_publisher_init( + &publisher_3, this->node_ptr, ts, topic_3, + &publisher_3_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() override + { + rcl_ret_t ret = rcl_publisher_fini(&publisher_1, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_publisher_fini(&publisher_2, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_publisher_fini(&publisher_3, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + CLASSNAME(TestNetworkFlowEndpointsNode, RMW_IMPLEMENTATION) ::TearDown(); + } +}; + +class CLASSNAME (TestSubscriptionNetworkFlowEndpoints, RMW_IMPLEMENTATION) + : public CLASSNAME(TestNetworkFlowEndpointsNode, RMW_IMPLEMENTATION) +{ +public: + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + const char * topic_1 = "chatter"; + const char * topic_2 = "mutter"; + const char * topic_3 = "sing"; + + rcl_subscription_t subscription_1; + rcl_subscription_t subscription_2; + rcl_subscription_t subscription_3; + + rcl_subscription_options_t subscription_1_options; + rcl_subscription_options_t subscription_2_options; + rcl_subscription_options_t subscription_3_options; + + void SetUp() override + { + CLASSNAME(TestNetworkFlowEndpointsNode, RMW_IMPLEMENTATION) ::SetUp(); + + subscription_1 = rcl_get_zero_initialized_subscription(); + subscription_1_options = rcl_subscription_get_default_options(); + rcl_ret_t ret = rcl_subscription_init( + &subscription_1, this->node_ptr, ts, topic_1, &subscription_1_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + subscription_2 = rcl_get_zero_initialized_subscription(); + subscription_2_options = rcl_subscription_get_default_options(); + subscription_2_options.rmw_subscription_options.require_unique_network_flow_endpoints = + RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_STRICTLY_REQUIRED; + ret = rcl_subscription_init( + &subscription_2, this->node_ptr, ts, topic_2, + &subscription_2_options); + ASSERT_TRUE( + ret == RCL_RET_OK || ret == RCL_RET_UNSUPPORTED || + ret == RCL_RET_ERROR) << rcl_get_error_string().str; + rcl_reset_error(); + + subscription_3 = rcl_get_zero_initialized_subscription(); + subscription_3_options = rcl_subscription_get_default_options(); + subscription_3_options.rmw_subscription_options.require_unique_network_flow_endpoints = + RMW_UNIQUE_NETWORK_FLOW_ENDPOINTS_OPTIONALLY_REQUIRED; + ret = rcl_subscription_init( + &subscription_3, this->node_ptr, ts, topic_3, + &subscription_3_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() override + { + rcl_ret_t ret = rcl_subscription_fini(&subscription_1, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_subscription_fini(&subscription_2, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_subscription_fini(&subscription_3, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + CLASSNAME(TestNetworkFlowEndpointsNode, RMW_IMPLEMENTATION) ::TearDown(); + } +}; + +TEST_F( + CLASSNAME( + TestPublisherNetworkFlowEndpoints, + RMW_IMPLEMENTATION), test_publisher_get_network_flow_endpoints_errors) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t failing_allocator = get_failing_allocator(); + rcl_network_flow_endpoint_array_t network_flow_endpoint_array = + rcl_get_zero_initialized_network_flow_endpoint_array(); + + // Invalid publisher + ret = rcl_publisher_get_network_flow_endpoints( + nullptr, &allocator, &network_flow_endpoint_array); + EXPECT_TRUE(ret == RCL_RET_INVALID_ARGUMENT || ret == RCL_RET_UNSUPPORTED); + rcl_reset_error(); + + // Invalid allocator + ret = rcl_publisher_get_network_flow_endpoints( + &this->publisher_1, nullptr, &network_flow_endpoint_array); + EXPECT_TRUE(ret == RCL_RET_INVALID_ARGUMENT || ret == RCL_RET_UNSUPPORTED); + rcl_reset_error(); + + // Invalid network_flow_endpoint_array + ret = rcl_publisher_get_network_flow_endpoints( + &this->publisher_1, &allocator, nullptr); + EXPECT_TRUE(ret == RCL_RET_INVALID_ARGUMENT || ret == RCL_RET_UNSUPPORTED); + rcl_reset_error(); + + // Failing allocator + set_failing_allocator_is_failing(failing_allocator, true); + ret = rcl_publisher_get_network_flow_endpoints( + &this->publisher_1, &failing_allocator, &network_flow_endpoint_array); + EXPECT_TRUE(ret == RCL_RET_BAD_ALLOC || ret == RCL_RET_UNSUPPORTED); + rcl_reset_error(); + + // Non-zero network_flow_endpoint_array + network_flow_endpoint_array.size = 1; + ret = rcl_publisher_get_network_flow_endpoints( + &this->publisher_1, &allocator, &network_flow_endpoint_array); + EXPECT_TRUE(ret == RCL_RET_ERROR || ret == RCL_RET_UNSUPPORTED); + rcl_reset_error(); +} + +TEST_F( + CLASSNAME( + TestPublisherNetworkFlowEndpoints, + RMW_IMPLEMENTATION), test_publisher_get_network_flow_endpoints) { + rcl_ret_t ret_1; + rcl_ret_t ret_2; + rcl_allocator_t allocator = rcl_get_default_allocator(); + + // Get network flow endpoints of ordinary publisher + rcl_network_flow_endpoint_array_t network_flow_endpoint_array_1 = + rcl_get_zero_initialized_network_flow_endpoint_array(); + ret_1 = rcl_publisher_get_network_flow_endpoints( + &this->publisher_1, &allocator, &network_flow_endpoint_array_1); + EXPECT_TRUE(ret_1 == RCL_RET_OK || ret_1 == RCL_RET_UNSUPPORTED); + + // Get network flow endpoints of publisher with unique network flow endpoints + rcl_network_flow_endpoint_array_t network_flow_endpoint_array_2 = + rcl_get_zero_initialized_network_flow_endpoint_array(); + if (rcl_publisher_is_valid(&this->publisher_2)) { + rcl_network_flow_endpoint_array_t network_flow_endpoint_array_2 = + rcl_get_zero_initialized_network_flow_endpoint_array(); + ret_2 = rcl_publisher_get_network_flow_endpoints( + &this->publisher_2, &allocator, &network_flow_endpoint_array_2); + EXPECT_TRUE(ret_2 == RCL_RET_OK || ret_2 == RCL_RET_UNSUPPORTED); + } else { + ret_2 = RCL_RET_ERROR; + } + + if (ret_1 == RCL_RET_OK && ret_2 == RCL_RET_OK) { + // Expect network flow endpoints not to be same + for (size_t i = 0; i < network_flow_endpoint_array_1.size; i++) { + for (size_t j = 0; j < network_flow_endpoint_array_2.size; j++) { + bool strcmp_ret = false; + if (strcmp( + network_flow_endpoint_array_1.network_flow_endpoint[i].internet_address, + network_flow_endpoint_array_2.network_flow_endpoint[j].internet_address) == 0) + { + strcmp_ret = true; + } + EXPECT_FALSE( + network_flow_endpoint_array_1.network_flow_endpoint[i].transport_protocol == + network_flow_endpoint_array_2.network_flow_endpoint[j].transport_protocol && + network_flow_endpoint_array_1.network_flow_endpoint[i].internet_protocol == + network_flow_endpoint_array_2.network_flow_endpoint[j].internet_protocol && + network_flow_endpoint_array_1.network_flow_endpoint[i].transport_port == + network_flow_endpoint_array_2.network_flow_endpoint[j].transport_port && + network_flow_endpoint_array_1.network_flow_endpoint[i].flow_label == + network_flow_endpoint_array_2.network_flow_endpoint[j].flow_label && + network_flow_endpoint_array_1.network_flow_endpoint[i].dscp == + network_flow_endpoint_array_2.network_flow_endpoint[j].dscp && + strcmp_ret); + } + } + } + + // Release resources + rcl_network_flow_endpoint_array_fini(&network_flow_endpoint_array_1); + rcl_network_flow_endpoint_array_fini(&network_flow_endpoint_array_2); +} + +TEST_F( + CLASSNAME( + TestSubscriptionNetworkFlowEndpoints, + RMW_IMPLEMENTATION), test_subscription_get_network_flow_endpoints_errors) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t failing_allocator = get_failing_allocator(); + rcl_network_flow_endpoint_array_t network_flow_endpoint_array = + rcl_get_zero_initialized_network_flow_endpoint_array(); + + // Invalid subscription + ret = rcl_subscription_get_network_flow_endpoints( + nullptr, &allocator, &network_flow_endpoint_array); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + // Invalid allocator + ret = rcl_subscription_get_network_flow_endpoints( + &this->subscription_1, nullptr, &network_flow_endpoint_array); + EXPECT_TRUE(ret == RCL_RET_INVALID_ARGUMENT || ret == RCL_RET_UNSUPPORTED); + rcl_reset_error(); + + // Invalid network_flow_endpoint_array + ret = rcl_subscription_get_network_flow_endpoints( + &this->subscription_1, &allocator, nullptr); + EXPECT_TRUE(ret == RCL_RET_INVALID_ARGUMENT || ret == RCL_RET_UNSUPPORTED); + rcl_reset_error(); + + // Failing allocator + set_failing_allocator_is_failing(failing_allocator, true); + ret = rcl_subscription_get_network_flow_endpoints( + &this->subscription_1, &failing_allocator, &network_flow_endpoint_array); + EXPECT_TRUE(ret == RCL_RET_BAD_ALLOC || ret == RCL_RET_UNSUPPORTED); + rcl_reset_error(); + + // Non-zero network_flow_endpoint_array + network_flow_endpoint_array.size = 1; + ret = rcl_subscription_get_network_flow_endpoints( + &this->subscription_1, &allocator, &network_flow_endpoint_array); + EXPECT_TRUE(ret == RCL_RET_ERROR || ret == RCL_RET_UNSUPPORTED); + rcl_reset_error(); +} + +TEST_F( + CLASSNAME( + TestSubscriptionNetworkFlowEndpoints, + RMW_IMPLEMENTATION), test_subscription_get_network_flow_endpoints) { + rcl_ret_t ret_1; + rcl_ret_t ret_2; + rcl_allocator_t allocator = rcl_get_default_allocator(); + + // Get network flow endpoints of ordinary subscription + rcl_network_flow_endpoint_array_t network_flow_endpoint_array_1 = + rcl_get_zero_initialized_network_flow_endpoint_array(); + ret_1 = rcl_subscription_get_network_flow_endpoints( + &this->subscription_1, &allocator, &network_flow_endpoint_array_1); + EXPECT_TRUE(ret_1 == RCL_RET_OK || ret_1 == RCL_RET_UNSUPPORTED); + + // Get network flow endpoints of subscription with unique network flow endpoints + rcl_network_flow_endpoint_array_t network_flow_endpoint_array_2 = + rcl_get_zero_initialized_network_flow_endpoint_array(); + if (rcl_subscription_is_valid(&this->subscription_2)) { + rcl_network_flow_endpoint_array_t network_flow_endpoint_array_2 = + rcl_get_zero_initialized_network_flow_endpoint_array(); + ret_2 = rcl_subscription_get_network_flow_endpoints( + &this->subscription_2, &allocator, &network_flow_endpoint_array_2); + EXPECT_TRUE(ret_2 == RCL_RET_OK || ret_2 == RCL_RET_UNSUPPORTED); + } else { + ret_2 = RCL_RET_ERROR; + } + + if (ret_1 == RCL_RET_OK && ret_2 == RCL_RET_OK) { + // Expect network flow endpoints not to be same + for (size_t i = 0; i < network_flow_endpoint_array_1.size; i++) { + for (size_t j = 0; j < network_flow_endpoint_array_2.size; j++) { + bool strcmp_ret = false; + if (strcmp( + network_flow_endpoint_array_1.network_flow_endpoint[i].internet_address, + network_flow_endpoint_array_2.network_flow_endpoint[j].internet_address) == 0) + { + strcmp_ret = true; + } + EXPECT_FALSE( + network_flow_endpoint_array_1.network_flow_endpoint[i].transport_protocol == + network_flow_endpoint_array_2.network_flow_endpoint[j].transport_protocol && + network_flow_endpoint_array_1.network_flow_endpoint[i].internet_protocol == + network_flow_endpoint_array_2.network_flow_endpoint[j].internet_protocol && + network_flow_endpoint_array_1.network_flow_endpoint[i].transport_port == + network_flow_endpoint_array_2.network_flow_endpoint[j].transport_port && + network_flow_endpoint_array_1.network_flow_endpoint[i].flow_label == + network_flow_endpoint_array_2.network_flow_endpoint[j].flow_label && + network_flow_endpoint_array_1.network_flow_endpoint[i].dscp == + network_flow_endpoint_array_2.network_flow_endpoint[j].dscp && + strcmp_ret); + } + } + } + + // Release resources + rcl_network_flow_endpoint_array_fini(&network_flow_endpoint_array_1); + rcl_network_flow_endpoint_array_fini(&network_flow_endpoint_array_2); +} diff --git a/test/rcl/test_node.cpp b/test/rcl/test_node.cpp new file mode 100644 index 0000000..716fb5f --- /dev/null +++ b/test/rcl/test_node.cpp @@ -0,0 +1,1045 @@ +// Copyright 2015 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 "rcl/rcl.h" +#include "rcl/node.h" +#include "rmw/rmw.h" // For rmw_get_implementation_identifier. +#include "rmw/validate_namespace.h" +#include "rmw/validate_node_name.h" + +#include "./failing_allocator_functions.hpp" +#include "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp" +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcutils/env.h" +#include "rcutils/testing/fault_injection.h" +#include "rcl/error_handling.h" +#include "rcl/logging.h" +#include "rcl/logging_rosout.h" + +#include "../mocking_utils/patch.hpp" +#include "./arg_macros.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +using osrf_testing_tools_cpp::memory_tools::on_unexpected_malloc; +using osrf_testing_tools_cpp::memory_tools::on_unexpected_realloc; +using osrf_testing_tools_cpp::memory_tools::on_unexpected_calloc; +using osrf_testing_tools_cpp::memory_tools::on_unexpected_free; + +bool operator==( + const rmw_time_t & lhs, + const rmw_time_t & rhs) +{ + return lhs.sec == rhs.sec && lhs.nsec == rhs.nsec; +} + +bool operator==( + const rmw_qos_profile_t & lhs, + const rmw_qos_profile_t & rhs) +{ + return lhs.history == rhs.history && + lhs.depth == rhs.depth && + lhs.reliability == rhs.reliability && + lhs.durability == rhs.durability && + lhs.deadline == rhs.deadline && + lhs.lifespan == rhs.lifespan && + lhs.liveliness == rhs.liveliness && + lhs.liveliness_lease_duration == rhs.liveliness_lease_duration && + lhs.avoid_ros_namespace_conventions == rhs.avoid_ros_namespace_conventions; +} + +class CLASSNAME (TestNodeFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + void SetUp() + { + auto common = + [](auto service, const char * name) { + // only fail if call originated in our library, librcl. + std::regex pattern("/?librcl\\."); + auto st = service.get_stack_trace(); // nullptr if stack trace not available + if (st && st->matches_any_object_filename(pattern)) { + // Implicitly this means if one of the rmw implementations uses threads + // and does memory allocations in them, but the calls didn't originate + // from an rcl call, we will ignore it. + // The goal here is ensure that no rcl function or thread is using memory. + // Separate tests will be needed to ensure the rmw implementation does + // not allocate memory or cause it to be allocated. + service.print_backtrace(); + ADD_FAILURE() << "Unexpected call to " << name << " originating from within librcl."; + } + }; + osrf_testing_tools_cpp::memory_tools::initialize(); + on_unexpected_malloc([common](auto service) {common(service, "malloc");}); + on_unexpected_realloc([common](auto service) {common(service, "realloc");}); + on_unexpected_calloc([common](auto service) {common(service, "calloc");}); + on_unexpected_free([common](auto service) {common(service, "free");}); + } + + void TearDown() + { + osrf_testing_tools_cpp::memory_tools::uninitialize(); + } +}; + +/* Tests the node accessors, i.e. rcl_node_get_* functions. + */ +TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_accessors) { + osrf_testing_tools_cpp::memory_tools::enable_monitoring_in_all_threads(); + rcl_ret_t ret; + // Initialize rcl with rcl_init(). + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + EXPECT_EQ(RCL_RET_OK, rcl_init_options_set_domain_id(&init_options, 42)); + rcl_context_t invalid_context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &invalid_context); + ASSERT_EQ(RCL_RET_OK, ret); // Shutdown later after invalid node. + // Create an invalid node (rcl_shutdown). + rcl_node_t invalid_node = rcl_get_zero_initialized_node(); + const char * name = "test_rcl_node_accessors_node"; + const char * namespace_ = "/ns"; + const char * fq_name = "/ns/test_rcl_node_accessors_node"; + rcl_node_options_t default_options = rcl_node_get_default_options(); + ret = rcl_node_init(&invalid_node, name, namespace_, &invalid_context, &default_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + osrf_testing_tools_cpp::memory_tools::disable_monitoring_in_all_threads(); + rcl_ret_t ret = rcl_node_fini(&invalid_node); + EXPECT_EQ(RCL_RET_OK, rcl_context_fini(&invalid_context)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, ret); + }); + ret = rcl_shutdown(&invalid_context); // Shutdown to invalidate the node. + ASSERT_EQ(RCL_RET_OK, ret); + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + osrf_testing_tools_cpp::memory_tools::disable_monitoring_in_all_threads(); + ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context)); + ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context)); + }); + // Create a zero init node. + rcl_node_t zero_node = rcl_get_zero_initialized_node(); + // Create a normal node. + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, name, namespace_, &context, &default_options); + ASSERT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + osrf_testing_tools_cpp::memory_tools::disable_monitoring_in_all_threads(); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + }); + // Test rcl_node_is_valid(). + bool is_valid; + is_valid = rcl_node_is_valid(nullptr); + EXPECT_FALSE(is_valid); + rcl_reset_error(); + is_valid = rcl_node_is_valid(&zero_node); + EXPECT_FALSE(is_valid); + rcl_reset_error(); + + // invalid node will be true for rcl_node_is_valid_except_context, but false for valid only + is_valid = rcl_node_is_valid_except_context(&invalid_node); + EXPECT_TRUE(is_valid); + rcl_reset_error(); + is_valid = rcl_node_is_valid(&invalid_node); + EXPECT_FALSE(is_valid); + rcl_reset_error(); + + is_valid = rcl_node_is_valid(&node); + EXPECT_TRUE(is_valid); + rcl_reset_error(); + // Test rcl_node_get_name(). + const char * actual_node_name; + actual_node_name = rcl_node_get_name(nullptr); + EXPECT_EQ(nullptr, actual_node_name); + rcl_reset_error(); + actual_node_name = rcl_node_get_name(&zero_node); + EXPECT_EQ(nullptr, actual_node_name); + rcl_reset_error(); + actual_node_name = rcl_node_get_name(&invalid_node); + EXPECT_STREQ(name, actual_node_name); + rcl_reset_error(); + EXPECT_NO_MEMORY_OPERATIONS( + { + actual_node_name = rcl_node_get_name(&node); + }); + EXPECT_TRUE(actual_node_name ? true : false); + if (actual_node_name) { + EXPECT_STREQ(name, actual_node_name); + } + // Test rcl_node_get_namespace(). + const char * actual_node_namespace; + actual_node_namespace = rcl_node_get_namespace(nullptr); + EXPECT_EQ(nullptr, actual_node_namespace); + rcl_reset_error(); + actual_node_namespace = rcl_node_get_namespace(&zero_node); + EXPECT_EQ(nullptr, actual_node_namespace); + rcl_reset_error(); + actual_node_namespace = rcl_node_get_namespace(&invalid_node); + EXPECT_STREQ(namespace_, actual_node_namespace); + rcl_reset_error(); + EXPECT_NO_MEMORY_OPERATIONS( + { + actual_node_namespace = rcl_node_get_namespace(&node); + }); + EXPECT_STREQ(namespace_, actual_node_namespace); + // Test rcl_node_get_fully_qualified_name(). + const char * actual_fq_node_name; + actual_fq_node_name = rcl_node_get_fully_qualified_name(nullptr); + EXPECT_EQ(nullptr, actual_fq_node_name); + rcl_reset_error(); + actual_fq_node_name = rcl_node_get_fully_qualified_name(&zero_node); + EXPECT_EQ(nullptr, actual_fq_node_name); + rcl_reset_error(); + actual_fq_node_name = rcl_node_get_fully_qualified_name(&invalid_node); + EXPECT_STREQ(fq_name, actual_fq_node_name); + rcl_reset_error(); + EXPECT_NO_MEMORY_OPERATIONS( + { + actual_fq_node_name = rcl_node_get_fully_qualified_name(&node); + }); + EXPECT_STREQ(fq_name, actual_fq_node_name); + // Test rcl_node_get_logger_name(). + const char * actual_node_logger_name; + actual_node_logger_name = rcl_node_get_logger_name(nullptr); + EXPECT_EQ(nullptr, actual_node_logger_name); + rcl_reset_error(); + actual_node_logger_name = rcl_node_get_logger_name(&zero_node); + EXPECT_EQ(nullptr, actual_node_logger_name); + rcl_reset_error(); + actual_node_logger_name = rcl_node_get_logger_name(&invalid_node); + EXPECT_NE(actual_node_logger_name, nullptr); + if (actual_node_logger_name) { + EXPECT_EQ("ns." + std::string(name), std::string(actual_node_logger_name)); + } + rcl_reset_error(); + EXPECT_NO_MEMORY_OPERATIONS( + { + actual_node_logger_name = rcl_node_get_logger_name(&node); + }); + EXPECT_NE(actual_node_logger_name, nullptr); + if (actual_node_logger_name) { + EXPECT_EQ("ns." + std::string(name), std::string(actual_node_logger_name)); + } + // Test rcl_node_get_options(). + const rcl_node_options_t * actual_options; + actual_options = rcl_node_get_options(nullptr); + EXPECT_EQ(nullptr, actual_options); + rcl_reset_error(); + actual_options = rcl_node_get_options(&zero_node); + EXPECT_EQ(nullptr, actual_options); + rcl_reset_error(); + actual_options = rcl_node_get_options(&invalid_node); + EXPECT_NE(nullptr, actual_options); + if (actual_options) { + EXPECT_EQ(default_options.allocator.allocate, actual_options->allocator.allocate); + } + rcl_reset_error(); + EXPECT_NO_MEMORY_OPERATIONS( + { + actual_options = rcl_node_get_options(&node); + }); + EXPECT_NE(nullptr, actual_options); + if (actual_options) { + EXPECT_EQ(default_options.allocator.allocate, actual_options->allocator.allocate); + } + // Test rcl_node_get_domain_id(). + size_t actual_domain_id; + ret = rcl_node_get_domain_id(nullptr, &actual_domain_id); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + ret = rcl_node_get_domain_id(&zero_node, &actual_domain_id); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + ret = rcl_node_get_domain_id(&invalid_node, &actual_domain_id); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret); + rcl_reset_error(); + EXPECT_NO_MEMORY_OPERATIONS( + { + ret = rcl_node_get_domain_id(&node, &actual_domain_id); + }); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(42u, actual_domain_id); + actual_domain_id = 0u; + EXPECT_NO_MEMORY_OPERATIONS( + { + ret = rcl_context_get_domain_id(&context, &actual_domain_id); + }); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(42u, actual_domain_id); + + // Test rcl_node_get_rmw_handle(). + rmw_node_t * node_handle; + node_handle = rcl_node_get_rmw_handle(nullptr); + EXPECT_EQ(nullptr, node_handle); + rcl_reset_error(); + node_handle = rcl_node_get_rmw_handle(&zero_node); + EXPECT_EQ(nullptr, node_handle); + rcl_reset_error(); + node_handle = rcl_node_get_rmw_handle(&invalid_node); + EXPECT_NE(nullptr, node_handle); + rcl_reset_error(); + EXPECT_NO_MEMORY_OPERATIONS( + { + node_handle = rcl_node_get_rmw_handle(&node); + }); + EXPECT_NE(nullptr, node_handle); + // Test rcl_node_get_rcl_instance_id(). + uint64_t instance_id; + instance_id = rcl_node_get_rcl_instance_id(nullptr); + EXPECT_EQ(0u, instance_id); + rcl_reset_error(); + instance_id = rcl_node_get_rcl_instance_id(&zero_node); + EXPECT_EQ(0u, instance_id); + rcl_reset_error(); + instance_id = rcl_node_get_rcl_instance_id(&invalid_node); + EXPECT_EQ(0u, instance_id); + rcl_reset_error(); + EXPECT_NO_MEMORY_OPERATIONS( + { + instance_id = rcl_node_get_rcl_instance_id(&node); + }); + EXPECT_NE(0u, instance_id); + // Test rcl_node_get_graph_guard_condition + const rcl_guard_condition_t * graph_guard_condition; + graph_guard_condition = rcl_node_get_graph_guard_condition(nullptr); + EXPECT_EQ(nullptr, graph_guard_condition); + rcl_reset_error(); + graph_guard_condition = rcl_node_get_graph_guard_condition(&zero_node); + EXPECT_EQ(nullptr, graph_guard_condition); + rcl_reset_error(); + graph_guard_condition = rcl_node_get_graph_guard_condition(&invalid_node); + EXPECT_NE(nullptr, graph_guard_condition); + rcl_reset_error(); + EXPECT_NO_MEMORY_OPERATIONS( + { + graph_guard_condition = rcl_node_get_graph_guard_condition(&node); + }); + EXPECT_NE(nullptr, graph_guard_condition); +} + +/* Tests the node life cycle, including rcl_node_init() and rcl_node_fini(). + */ +TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_life_cycle) { + rcl_ret_t ret; + rcl_context_t context = rcl_get_zero_initialized_context(); + rcl_node_t node = rcl_get_zero_initialized_node(); + const char * name = "test_rcl_node_life_cycle_node"; + const char * namespace_ = "/ns"; + rcl_node_options_t default_options = rcl_node_get_default_options(); + // Trying to init before rcl_init() should fail. + ret = rcl_node_init(&node, name, "", &context, &default_options); + ASSERT_EQ(RCL_RET_NOT_INIT, ret) << "Expected RCL_RET_NOT_INIT"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Initialize rcl with rcl_init(). + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context)); + ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context)); + }); + // Try invalid arguments. + ret = rcl_node_init(nullptr, name, namespace_, &context, &default_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + ret = rcl_node_init(&node, nullptr, namespace_, &context, &default_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + ret = rcl_node_init(&node, name, nullptr, &context, &default_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + ret = rcl_node_init(&node, name, namespace_, nullptr, &default_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + ret = rcl_node_init(&node, name, namespace_, &context, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Try fini with invalid arguments. + ret = rcl_node_fini(nullptr); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << "Expected RCL_RET_NODE_INVALID"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Try fini with an uninitialized node. + ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + // Try a normal init and fini. + ret = rcl_node_init(&node, name, namespace_, &context, &default_options); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + // Try repeated init and fini calls. + ret = rcl_node_init(&node, name, namespace_, &context, &default_options); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_node_init(&node, name, namespace_, &context, &default_options); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << "Expected RCL_RET_ALREADY_INIT"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); +} + +TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_init_with_internal_errors) { + rcl_ret_t ret; + rcl_context_t context = rcl_get_zero_initialized_context(); + rcl_node_t node = rcl_get_zero_initialized_node(); + const char * name = "test_rcl_node_init_with_internal_errors"; + const char * namespace_ = "ns"; // force non-absolute namespace handling + rcl_node_options_t options = rcl_node_get_default_options(); + options.enable_rosout = true; // enable logging to cover more ground + // Initialize rcl with rcl_init(). + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_init_options_init(&init_options, allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_shutdown(&context)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_context_fini(&context)) << rcl_get_error_string().str; + }); + // Initialize logging and rosout. + ret = rcl_logging_configure(&context.global_arguments, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_logging_fini()) << rcl_get_error_string().str; + }); + ret = rcl_logging_rosout_init(&allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_logging_rosout_fini()) << rcl_get_error_string().str; + }); + // Try with invalid allocator. + rcl_node_options_t options_with_invalid_allocator = rcl_node_get_default_options(); + options_with_invalid_allocator.allocator.allocate = nullptr; + options_with_invalid_allocator.allocator.deallocate = nullptr; + options_with_invalid_allocator.allocator.reallocate = nullptr; + ret = rcl_node_init(&node, name, namespace_, &context, &options_with_invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << "Expected RCL_RET_INVALID_ARGUMENT"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Try with failing allocator. + rcl_node_options_t options_with_failing_allocator = rcl_node_get_default_options(); + options_with_failing_allocator.allocator.allocate = failing_malloc; + options_with_failing_allocator.allocator.reallocate = failing_realloc; + ret = rcl_node_init(&node, name, namespace_, &context, &options_with_failing_allocator); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << "Expected RCL_RET_BAD_ALLOC"; + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + // Try init but force internal errors. + { + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_create_node, nullptr); + ret = rcl_node_init(&node, name, namespace_, &context, &options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_node_get_graph_guard_condition, nullptr); + ret = rcl_node_init(&node, name, namespace_, &context, &options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_validate_node_name, RMW_RET_ERROR); + ret = rcl_node_init(&node, name, namespace_, &context, &options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_validate_namespace, RMW_RET_ERROR); + ret = rcl_node_init(&node, name, namespace_, &context, &options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + // Try normal init but force an internal error on fini. + { + ret = rcl_node_init(&node, name, namespace_, &context, &options); + EXPECT_EQ(RCL_RET_OK, ret); + auto mock = mocking_utils::inject_on_return("lib:rcl", rmw_destroy_node, RMW_RET_ERROR); + ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + + // Battle test node init. + RCUTILS_FAULT_INJECTION_TEST( + { + ret = rcl_node_init(&node, name, namespace_, &context, &options); + + int64_t count = rcutils_fault_injection_get_count(); + rcutils_fault_injection_set_count(RCUTILS_FAULT_INJECTION_NEVER_FAIL); + + if (RCL_RET_OK == ret) { + ASSERT_TRUE(rcl_node_is_valid(&node)); + EXPECT_EQ(RCL_RET_OK, rcl_node_fini(&node)) << rcl_get_error_string().str; + } else { + ASSERT_FALSE(rcl_node_is_valid(&node)); + rcl_reset_error(); + } + + rcutils_fault_injection_set_count(count); + }); +} + +/* Tests the node name restrictions enforcement. + */ +TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_name_restrictions) { + rcl_ret_t ret; + + // Initialize rcl with rcl_init(). + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context)); + ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context)); + }); + + const char * namespace_ = "/ns"; + rcl_node_options_t default_options = rcl_node_get_default_options(); + + // First do a normal node name. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, "my_node_42", namespace_, &context, &default_options); + ASSERT_EQ(RCL_RET_OK, ret); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + + // Node name with invalid characters. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, "my_node_42$", namespace_, &context, &default_options); + ASSERT_EQ(RCL_RET_NODE_INVALID_NAME, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + + // Node name with /, which is valid in a topic, but not a node name. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, "my/node_42", namespace_, &context, &default_options); + ASSERT_EQ(RCL_RET_NODE_INVALID_NAME, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + + // Node name with {}, which is valid in a topic, but not a node name. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, "my_{node}_42", namespace_, &context, &default_options); + ASSERT_EQ(RCL_RET_NODE_INVALID_NAME, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } +} + +/* Tests the node namespace restrictions enforcement. + */ +TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_namespace_restrictions) { + rcl_ret_t ret; + + // Initialize rcl with rcl_init(). + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context)); + ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context)); + }); + + const char * name = "node"; + rcl_node_options_t default_options = rcl_node_get_default_options(); + + // First do a normal node namespace. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, name, "/ns", &context, &default_options); + ASSERT_EQ(RCL_RET_OK, ret); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + + // Node namespace which is an empty string, which is also valid. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, name, "", &context, &default_options); + ASSERT_EQ(RCL_RET_OK, ret); + ASSERT_STREQ("/", rcl_node_get_namespace(&node)); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + + // Node namespace which is just a forward slash, which is valid. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, name, "/", &context, &default_options); + ASSERT_EQ(RCL_RET_OK, ret); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + + // Node namespaces with invalid characters. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, name, "/ns/{name}", &context, &default_options); + ASSERT_EQ(RCL_RET_NODE_INVALID_NAMESPACE, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, name, "/~/", &context, &default_options); + ASSERT_EQ(RCL_RET_NODE_INVALID_NAMESPACE, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + + // Node namespace with a trailing / which is not allowed. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, name, "/ns/foo/", &context, &default_options); + ASSERT_EQ(RCL_RET_NODE_INVALID_NAMESPACE, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + + // Node namespace which is not absolute, it should get / added automatically. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, name, "ns", &context, &default_options); + ASSERT_EQ(RCL_RET_OK, ret); + ASSERT_STREQ("/ns", rcl_node_get_namespace(&node)); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + + // Other reasons for being invalid, which are related to being part of a topic. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, name, "/starts/with/42number", &context, &default_options); + ASSERT_EQ(RCL_RET_NODE_INVALID_NAMESPACE, ret); + ASSERT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } +} + +/* Tests the logger name as well as fully qualified name associated with the node. + */ +TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_names) { + rcl_ret_t ret; + + // Initialize rcl with rcl_init(). + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context)); + ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context)); + }); + + const char * actual_node_logger_name; + const char * actual_node_name; + const char * actual_node_namespace; + const char * actual_node_fq_name; + + rcl_node_options_t default_options = rcl_node_get_default_options(); + + // First do a normal node namespace. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, "node", "/ns", &context, &default_options); + ASSERT_EQ(RCL_RET_OK, ret); + + actual_node_logger_name = rcl_node_get_logger_name(&node); + actual_node_name = rcl_node_get_name(&node); + actual_node_namespace = rcl_node_get_namespace(&node); + actual_node_fq_name = rcl_node_get_fully_qualified_name(&node); + + EXPECT_STREQ("ns.node", actual_node_logger_name); + EXPECT_STREQ("node", actual_node_name); + EXPECT_STREQ("/ns", actual_node_namespace); + EXPECT_STREQ("/ns/node", actual_node_fq_name); + + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + + // Node namespace that is an empty string. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, "node", "", &context, &default_options); + ASSERT_EQ(RCL_RET_OK, ret); + + actual_node_logger_name = rcl_node_get_logger_name(&node); + actual_node_name = rcl_node_get_name(&node); + actual_node_namespace = rcl_node_get_namespace(&node); + actual_node_fq_name = rcl_node_get_fully_qualified_name(&node); + + EXPECT_STREQ("node", actual_node_logger_name); + EXPECT_STREQ("node", actual_node_name); + EXPECT_STREQ("/", actual_node_namespace); + EXPECT_STREQ("/node", actual_node_fq_name); + + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + + // Node namespace that is just a forward slash. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, "node", "/", &context, &default_options); + ASSERT_EQ(RCL_RET_OK, ret); + + actual_node_logger_name = rcl_node_get_logger_name(&node); + actual_node_name = rcl_node_get_name(&node); + actual_node_namespace = rcl_node_get_namespace(&node); + actual_node_fq_name = rcl_node_get_fully_qualified_name(&node); + + EXPECT_STREQ("node", actual_node_logger_name); + EXPECT_STREQ("node", actual_node_name); + EXPECT_STREQ("/", actual_node_namespace); + EXPECT_STREQ("/node", actual_node_fq_name); + + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + + // Node namespace that is not absolute. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, "node", "ns", &context, &default_options); + ASSERT_EQ(RCL_RET_OK, ret); + + actual_node_logger_name = rcl_node_get_logger_name(&node); + actual_node_name = rcl_node_get_name(&node); + actual_node_namespace = rcl_node_get_namespace(&node); + actual_node_fq_name = rcl_node_get_fully_qualified_name(&node); + + EXPECT_STREQ("ns.node", actual_node_logger_name); + EXPECT_STREQ("node", actual_node_name); + EXPECT_STREQ("/ns", actual_node_namespace); + EXPECT_STREQ("/ns/node", actual_node_fq_name); + + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } + + // Nested namespace. + { + rcl_node_t node = rcl_get_zero_initialized_node(); + ret = rcl_node_init(&node, "node", "/ns/sub_1/sub_2", &context, &default_options); + ASSERT_EQ(RCL_RET_OK, ret); + + actual_node_logger_name = rcl_node_get_logger_name(&node); + actual_node_name = rcl_node_get_name(&node); + actual_node_namespace = rcl_node_get_namespace(&node); + actual_node_fq_name = rcl_node_get_fully_qualified_name(&node); + + EXPECT_STREQ("ns.sub_1.sub_2.node", actual_node_logger_name); + EXPECT_STREQ("node", actual_node_name); + EXPECT_STREQ("/ns/sub_1/sub_2", actual_node_namespace); + EXPECT_STREQ("/ns/sub_1/sub_2/node", actual_node_fq_name); + + rcl_ret_t ret = rcl_node_fini(&node); + EXPECT_EQ(RCL_RET_OK, ret); + } +} + +/* Tests the node_options functionality + */ +TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_options) { + rcl_node_options_t default_options = rcl_node_get_default_options(); + rcl_node_options_t not_ini_options = rcl_node_get_default_options(); + memset(¬_ini_options.rosout_qos, 0, sizeof(rmw_qos_profile_t)); + + EXPECT_TRUE(default_options.use_global_arguments); + EXPECT_TRUE(default_options.enable_rosout); + EXPECT_EQ(rcl_qos_profile_rosout_default, default_options.rosout_qos); + EXPECT_TRUE(rcutils_allocator_is_valid(&(default_options.allocator))); + + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_node_options_copy(nullptr, &default_options)); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_node_options_copy(&default_options, nullptr)); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_node_options_copy(&default_options, &default_options)); + + const char * argv[] = { + "process_name", "--ros-args", "/foo/bar:=", "-r", "bar:=/fiz/buz", "}bar:=fiz", "--", "arg"}; + int argc = sizeof(argv) / sizeof(const char *); + EXPECT_EQ( + RCL_RET_OK, + rcl_parse_arguments(argc, argv, default_options.allocator, &(default_options.arguments))); + default_options.use_global_arguments = false; + default_options.enable_rosout = false; + default_options.rosout_qos = rmw_qos_profile_default; + EXPECT_EQ(RCL_RET_OK, rcl_node_options_copy(&default_options, ¬_ini_options)); + EXPECT_FALSE(not_ini_options.use_global_arguments); + EXPECT_FALSE(not_ini_options.enable_rosout); + EXPECT_EQ(default_options.rosout_qos, not_ini_options.rosout_qos); + EXPECT_EQ( + rcl_arguments_get_count_unparsed(&(default_options.arguments)), + rcl_arguments_get_count_unparsed(&(not_ini_options.arguments))); + EXPECT_EQ( + rcl_arguments_get_count_unparsed_ros(&(default_options.arguments)), + rcl_arguments_get_count_unparsed_ros(&(not_ini_options.arguments))); + + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_node_options_fini(nullptr)); + EXPECT_EQ(RCL_RET_OK, rcl_node_options_fini(&default_options)); + EXPECT_EQ(RCL_RET_OK, rcl_node_options_fini(¬_ini_options)); +} + +/* Tests special case node_options + */ +TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_options_fail) { + rcl_node_options_t prev_ini_options = rcl_node_get_default_options(); + const char * argv[] = {"--ros-args"}; + int argc = sizeof(argv) / sizeof(const char *); + EXPECT_EQ( + RCL_RET_OK, + rcl_parse_arguments(argc, argv, rcl_get_default_allocator(), &prev_ini_options.arguments)); + + rcl_node_options_t default_options = rcl_node_get_default_options(); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_node_options_copy(&default_options, &prev_ini_options)); + + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&prev_ini_options.arguments)); +} + +/* Tests special case node_options + */ +TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_node_resolve_name) { + rcl_allocator_t default_allocator = rcl_get_default_allocator(); + char * final_name = NULL; + rcl_node_t node = rcl_get_zero_initialized_node(); + // Invalid node + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_resolve_name(NULL, "my_topic", default_allocator, false, false, &final_name)); + EXPECT_EQ( + RCL_RET_ERROR, + rcl_node_resolve_name(&node, "my_topic", default_allocator, false, false, &final_name)); + + // Initialize rcl with rcl_init(). + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &context); + ASSERT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ASSERT_EQ(RCL_RET_OK, rcl_shutdown(&context)); + ASSERT_EQ(RCL_RET_OK, rcl_context_fini(&context)); + }); + + // Initialize node with default options + rcl_node_options_t options = rcl_node_get_default_options(); + rcl_arguments_t local_arguments = rcl_get_zero_initialized_arguments(); + const char * argv[] = {"process_name", "--ros-args", "-r", "/bar/foo:=/foo/local_args"}; + unsigned int argc = (sizeof(argv) / sizeof(const char *)); + ret = rcl_parse_arguments( + argc, argv, default_allocator, &local_arguments); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + options.arguments = local_arguments; // transfer ownership + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_node_options_fini(&options); + }); + ret = rcl_node_init(&node, "node", "/ns", &context, &options); + ASSERT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ASSERT_EQ(RCL_RET_OK, rcl_node_fini(&node)); + }); + + // Invalid arguments + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_resolve_name(&node, NULL, default_allocator, false, false, &final_name)); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_node_resolve_name(&node, "my_topic", default_allocator, false, false, NULL)); + + // Some valid options, test_remap and test_expand_topic_name already have good coverage + EXPECT_EQ( + RCL_RET_OK, + rcl_node_resolve_name(&node, "my_topic", default_allocator, false, false, &final_name)); + ASSERT_TRUE(final_name); + EXPECT_STREQ("/ns/my_topic", final_name); + default_allocator.deallocate(final_name, default_allocator.state); + + EXPECT_EQ( + RCL_RET_OK, + rcl_node_resolve_name(&node, "my_service", default_allocator, true, false, &final_name)); + ASSERT_TRUE(final_name); + EXPECT_STREQ("/ns/my_service", final_name); + default_allocator.deallocate(final_name, default_allocator.state); + + EXPECT_EQ( + RCL_RET_OK, + rcl_node_resolve_name(&node, "/bar/foo", default_allocator, false, false, &final_name)); + ASSERT_TRUE(final_name); + EXPECT_STREQ("/foo/local_args", final_name); + default_allocator.deallocate(final_name, default_allocator.state); + + EXPECT_EQ( + RCL_RET_OK, + rcl_node_resolve_name(&node, "/bar/foo", default_allocator, false, true, &final_name)); + ASSERT_TRUE(final_name); + EXPECT_STREQ("/bar/foo", final_name); + default_allocator.deallocate(final_name, default_allocator.state); + + EXPECT_EQ( + RCL_RET_OK, + rcl_node_resolve_name(&node, "relative_ns/foo", default_allocator, true, false, &final_name)); + ASSERT_TRUE(final_name); + EXPECT_STREQ("/ns/relative_ns/foo", final_name); + default_allocator.deallocate(final_name, default_allocator.state); +} + +/* Tests special case node_options + */ +TEST_F(CLASSNAME(TestNodeFixture, RMW_IMPLEMENTATION), test_rcl_get_disable_loaned_message) { + { + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_get_disable_loaned_message(nullptr)); + rcl_reset_error(); + } + + { + bool disable_loaned_message = false; + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_get_env, "internal error"); + EXPECT_EQ(RCL_RET_ERROR, rcl_get_disable_loaned_message(&disable_loaned_message)); + rcl_reset_error(); + } + + { + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "0")); + bool disable_loaned_message = true; + EXPECT_EQ(RCL_RET_OK, rcl_get_disable_loaned_message(&disable_loaned_message)); + EXPECT_FALSE(disable_loaned_message); + } + + { + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "1")); + bool disable_loaned_message = false; + EXPECT_EQ(RCL_RET_OK, rcl_get_disable_loaned_message(&disable_loaned_message)); + EXPECT_TRUE(disable_loaned_message); + } + + { + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "2")); + bool disable_loaned_message = true; + EXPECT_EQ(RCL_RET_OK, rcl_get_disable_loaned_message(&disable_loaned_message)); + EXPECT_FALSE(disable_loaned_message); + } + + { + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "11")); + bool disable_loaned_message = true; + EXPECT_EQ(RCL_RET_OK, rcl_get_disable_loaned_message(&disable_loaned_message)); + EXPECT_FALSE(disable_loaned_message); + } +} diff --git a/test/rcl/test_publisher.cpp b/test/rcl/test_publisher.cpp new file mode 100644 index 0000000..c77e784 --- /dev/null +++ b/test/rcl/test_publisher.cpp @@ -0,0 +1,934 @@ +// Copyright 2015 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 "rcl/publisher.h" + +#include "rcl/rcl.h" +#include "test_msgs/msg/basic_types.h" +#include "test_msgs/msg/strings.h" +#include "rosidl_runtime_c/string_functions.h" + +#include "mimick/mimick.h" +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rcl/node.h" +#include "rcutils/env.h" +#include "rmw/validate_full_topic_name.h" +#include "rmw/validate_node_name.h" + +#include "./failing_allocator_functions.hpp" +#include "./publisher_impl.h" +#include "../mocking_utils/patch.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestPublisherFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + constexpr char name[] = "test_publisher_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +class CLASSNAME (TestPublisherFixtureInit, RMW_IMPLEMENTATION) + : public CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION) +{ +public: + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + const char * topic_name = "chatter"; + rcl_publisher_t publisher; + rcl_publisher_options_t publisher_options; + + void SetUp() override + { + CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION) ::SetUp(); + publisher = rcl_get_zero_initialized_publisher(); + publisher_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = rcl_publisher_init( + &publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() override + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION) ::TearDown(); + } +}; + +/* Basic nominal test of a publisher. + */ +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_nominal) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "chatter"; + constexpr char expected_topic_name[] = "/chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + EXPECT_EQ(strcmp(rcl_publisher_get_topic_name(&publisher), expected_topic_name), 0); + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + msg.int64_value = 42; + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__BasicTypes__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +/* Basic nominal test of a publisher with a string. + */ +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_nominal_string) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic_name[] = "chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, "testing")); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +/* Test two publishers using different message types with the same basename. + * + * Regression test for https://github.com/ros2/rmw_connext/issues/234, where rmw_connext_cpp could + * not support publishers on topics with the same basename (but different namespaces) using + * different message types, because at the time partitions were used for implementing namespaces. + */ +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publishers_different_types) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts_int = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + const char * topic_name = "basename"; + const char * expected_topic_name = "/basename"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts_int, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + EXPECT_EQ(strcmp(rcl_publisher_get_topic_name(&publisher), expected_topic_name), 0); + + rcl_publisher_t publisher_in_namespace = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts_string = ROSIDL_GET_MSG_TYPE_SUPPORT( + test_msgs, msg, Strings); + topic_name = "namespace/basename"; + expected_topic_name = "/namespace/basename"; + ret = rcl_publisher_init( + &publisher_in_namespace, this->node_ptr, ts_string, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher_in_namespace, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + EXPECT_EQ(strcmp(rcl_publisher_get_topic_name(&publisher_in_namespace), expected_topic_name), 0); + + test_msgs__msg__BasicTypes msg_int; + test_msgs__msg__BasicTypes__init(&msg_int); + msg_int.int64_value = 42; + ret = rcl_publish(&publisher, &msg_int, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + test_msgs__msg__BasicTypes__fini(&msg_int); + + test_msgs__msg__Strings msg_string; + test_msgs__msg__Strings__init(&msg_string); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg_string.string_value, "testing")); + ret = rcl_publish(&publisher_in_namespace, &msg_string, nullptr); + test_msgs__msg__Strings__fini(&msg_string); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +/* Testing the publisher init and fini functions. + */ +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_init_fini) { + rcl_ret_t ret; + // Setup valid inputs. + rcl_publisher_t publisher; + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "chatter"; + rcl_publisher_options_t default_publisher_options = rcl_publisher_get_default_options(); + + // Check if null publisher is valid + EXPECT_FALSE(rcl_publisher_is_valid(nullptr)); + rcl_reset_error(); + + // Check if zero initialized node is valid + publisher = rcl_get_zero_initialized_publisher(); + EXPECT_FALSE(rcl_publisher_is_valid(&publisher)); + rcl_reset_error(); + + // Check that valid publisher is valid + publisher = rcl_get_zero_initialized_publisher(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &default_publisher_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_publisher_is_valid(&publisher)); + // Try init a publisher already init + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &default_publisher_options); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << rcl_get_error_string().str; + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Pass invalid node to fini + ret = rcl_publisher_fini(&publisher, nullptr); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Pass nullptr publisher to fini + ret = rcl_publisher_fini(nullptr, this->node_ptr); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing null for publisher in init. + ret = rcl_publisher_init(nullptr, this->node_ptr, ts, topic_name, &default_publisher_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing null for a node pointer in init. + publisher = rcl_get_zero_initialized_publisher(); + ret = rcl_publisher_init(&publisher, nullptr, ts, topic_name, &default_publisher_options); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing an invalid (uninitialized) node in init. + publisher = rcl_get_zero_initialized_publisher(); + rcl_node_t invalid_node = rcl_get_zero_initialized_node(); + ret = rcl_publisher_init(&publisher, &invalid_node, ts, topic_name, &default_publisher_options); + EXPECT_EQ(RCL_RET_NODE_INVALID, ret) << rcl_get_error_string().str; + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing null for the type support in init. + publisher = rcl_get_zero_initialized_publisher(); + ret = rcl_publisher_init( + &publisher, this->node_ptr, nullptr, topic_name, &default_publisher_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing null for the topic name in init. + publisher = rcl_get_zero_initialized_publisher(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, nullptr, &default_publisher_options); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing null for the options in init. + publisher = rcl_get_zero_initialized_publisher(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing options with an invalid allocate in allocator with init. + publisher = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t publisher_options_with_invalid_allocator; + publisher_options_with_invalid_allocator = rcl_publisher_get_default_options(); + publisher_options_with_invalid_allocator.allocator.allocate = nullptr; + ret = rcl_publisher_init( + &publisher, this->node_ptr, ts, topic_name, &publisher_options_with_invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // Try passing options with an invalid deallocate in allocator with init. + publisher = rcl_get_zero_initialized_publisher(); + publisher_options_with_invalid_allocator = rcl_publisher_get_default_options(); + publisher_options_with_invalid_allocator.allocator.deallocate = nullptr; + ret = rcl_publisher_init( + &publisher, this->node_ptr, ts, topic_name, &publisher_options_with_invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // An allocator with an invalid realloc will probably work (so we will not test it). + + // Try passing options with a failing allocator with init. + publisher = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t publisher_options_with_failing_allocator; + publisher_options_with_failing_allocator = rcl_publisher_get_default_options(); + publisher_options_with_failing_allocator.allocator.allocate = failing_malloc; + publisher_options_with_failing_allocator.allocator.reallocate = failing_realloc; + publisher_options_with_failing_allocator.allocator.zero_allocate = failing_calloc; + ret = rcl_publisher_init( + &publisher, this->node_ptr, ts, topic_name, &publisher_options_with_failing_allocator); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_loan) { + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic_name[] = "chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = + rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + test_msgs__msg__Strings * msg_loaned = nullptr; + test_msgs__msg__Strings ** msg_loaned_ptr = &msg_loaned; + if (rcl_publisher_can_loan_messages(&publisher)) { + EXPECT_EQ( + RCL_RET_OK, rcl_borrow_loaned_message( + &publisher, + ts, + reinterpret_cast(msg_loaned_ptr))); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&(msg_loaned->string_value), "testing")); + EXPECT_EQ( + RCL_RET_OK, rcl_publish_loaned_message( + &publisher, + msg_loaned, + nullptr)); + } +} + +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_publisher_loan_disable) { + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "pod_msg"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = + rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + if (rcl_publisher_can_loan_messages(&publisher)) { + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "0")); + EXPECT_TRUE(rcl_publisher_can_loan_messages(&publisher)); + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "1")); + EXPECT_FALSE(rcl_publisher_can_loan_messages(&publisher)); + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "2")); + EXPECT_TRUE(rcl_publisher_can_loan_messages(&publisher)); + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "Unexpected")); + EXPECT_TRUE(rcl_publisher_can_loan_messages(&publisher)); + } else { + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "0")); + EXPECT_FALSE(rcl_publisher_can_loan_messages(&publisher)); + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "1")); + EXPECT_FALSE(rcl_publisher_can_loan_messages(&publisher)); + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "2")); + EXPECT_FALSE(rcl_publisher_can_loan_messages(&publisher)); + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "Unexpected")); + EXPECT_FALSE(rcl_publisher_can_loan_messages(&publisher)); + } +} + +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_invalid_publisher) { + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic_name[] = "chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = + rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + const rcl_publisher_options_t * publisher_options_rcv = rcl_publisher_get_options(&publisher); + ASSERT_NE(nullptr, publisher_options_rcv); + EXPECT_EQ(rmw_qos_profile_default.reliability, publisher_options_rcv->qos.reliability); + EXPECT_EQ(rmw_qos_profile_default.history, publisher_options_rcv->qos.history); + EXPECT_EQ(rmw_qos_profile_default.depth, publisher_options_rcv->qos.depth); + EXPECT_EQ(rmw_qos_profile_default.durability, publisher_options_rcv->qos.durability); + EXPECT_TRUE(rcutils_allocator_is_valid(&(publisher_options_rcv->allocator))); + + rmw_publisher_t * pub_rmw_handle = rcl_publisher_get_rmw_handle(&publisher); + EXPECT_NE(nullptr, pub_rmw_handle); + + rcl_context_t * pub_context = rcl_publisher_get_context(&publisher); + EXPECT_TRUE(rcl_context_is_valid(pub_context)); + EXPECT_EQ(rcl_context_get_instance_id(context_ptr), rcl_context_get_instance_id(pub_context)); + + EXPECT_EQ(RCL_RET_OK, rcl_publisher_assert_liveliness(&publisher)); + + EXPECT_EQ(RCL_RET_OK, rcl_publisher_wait_for_all_acked(&publisher, 0)); + + size_t count_size; + test_msgs__msg__BasicTypes msg; + rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); + rcl_publisher_impl_t * saved_impl = publisher.impl; + rcl_context_t * saved_context = publisher.impl->context; + rmw_publisher_t * saved_rmw_handle = publisher.impl->rmw_handle; + rmw_publisher_allocation_t * null_allocation_is_valid_arg = nullptr; + + // Change internal context to nullptr + publisher.impl->context = nullptr; + EXPECT_TRUE(rcl_publisher_is_valid_except_context(&publisher)); + EXPECT_NE(nullptr, rcl_publisher_get_topic_name(&publisher)); + EXPECT_NE(nullptr, rcl_publisher_get_rmw_handle(&publisher)); + EXPECT_NE(nullptr, rcl_publisher_get_actual_qos(&publisher)); + EXPECT_NE(nullptr, rcl_publisher_get_options(&publisher)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_is_valid(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_context(&publisher)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_can_loan_messages(&publisher)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, rcl_publisher_get_subscription_count(&publisher, &count_size)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publisher_assert_liveliness(&publisher)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publisher_wait_for_all_acked(&publisher, 10000000)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publish(&publisher, &msg, null_allocation_is_valid_arg)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_publish_serialized_message(&publisher, &serialized_msg, null_allocation_is_valid_arg)); + rcl_reset_error(); + publisher.impl->context = saved_context; + + // nullptr arguments + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_publish(&publisher, nullptr, null_allocation_is_valid_arg)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_publish_serialized_message(&publisher, nullptr, null_allocation_is_valid_arg)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_publisher_get_subscription_count(&publisher, nullptr)); + rcl_reset_error(); + + // Change internal rmw_handle to nullptr + publisher.impl->rmw_handle = nullptr; + EXPECT_FALSE(rcl_publisher_is_valid_except_context(&publisher)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_is_valid(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_topic_name(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_rmw_handle(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_actual_qos(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_options(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_context(&publisher)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_can_loan_messages(&publisher)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, rcl_publisher_get_subscription_count(&publisher, &count_size)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publisher_assert_liveliness(&publisher)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publisher_wait_for_all_acked(&publisher, 10000000)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publish(&publisher, &msg, null_allocation_is_valid_arg)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_publish_serialized_message(&publisher, &serialized_msg, null_allocation_is_valid_arg)); + rcl_reset_error(); + publisher.impl->rmw_handle = saved_rmw_handle; + + // Change internal implementation to nullptr + publisher.impl = nullptr; + EXPECT_FALSE(rcl_publisher_is_valid_except_context(&publisher)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_is_valid(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_topic_name(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_rmw_handle(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_actual_qos(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_options(&publisher)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_context(&publisher)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_can_loan_messages(&publisher)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, rcl_publisher_get_subscription_count(&publisher, &count_size)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publisher_assert_liveliness(&publisher)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publisher_wait_for_all_acked(&publisher, 10000000)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publish(&publisher, &msg, null_allocation_is_valid_arg)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_publish_serialized_message(&publisher, &serialized_msg, null_allocation_is_valid_arg)); + rcl_reset_error(); + publisher.impl = saved_impl; + + // Null tests + EXPECT_FALSE(rcl_publisher_is_valid_except_context(nullptr)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_is_valid(nullptr)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_topic_name(nullptr)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_rmw_handle(nullptr)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_actual_qos(nullptr)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_options(nullptr)); + rcl_reset_error(); + EXPECT_EQ(nullptr, rcl_publisher_get_context(nullptr)); + rcl_reset_error(); + EXPECT_FALSE(rcl_publisher_can_loan_messages(nullptr)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, rcl_publisher_get_subscription_count(nullptr, &count_size)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publisher_assert_liveliness(nullptr)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publisher_wait_for_all_acked(nullptr, 10000000)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_publish(nullptr, &msg, null_allocation_is_valid_arg)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_publish_serialized_message(nullptr, &serialized_msg, null_allocation_is_valid_arg)); + rcl_reset_error(); +} + +// Mocking rmw_publisher_count_matched_subscriptions to make +// rcl_publisher_get_subscription_count fail +TEST_F( + CLASSNAME(TestPublisherFixtureInit, RMW_IMPLEMENTATION), + test_mock_publisher_get_subscription_count) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_publisher_count_matched_subscriptions, RMW_RET_BAD_ALLOC); + + // Now normal usage of the function rcl_publisher_get_subscription_count returning + // unexpected RMW_RET_BAD_ALLOC + size_t count_size = 2u; + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_publisher_get_subscription_count(&publisher, &count_size)); + EXPECT_EQ(2u, count_size); + rcl_reset_error(); +} + +// Mocking rmw_publisher_assert_liveliness to make +// rcl_publisher_assert_liveliness fail +TEST_F(CLASSNAME(TestPublisherFixtureInit, RMW_IMPLEMENTATION), test_mock_assert_liveliness) { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_publisher_assert_liveliness, RMW_RET_ERROR); + + // Now normal usage of the function rcl_publisher_assert_liveliness returning + // unexpected RMW_RET_ERROR + EXPECT_EQ( + RCL_RET_ERROR, rcl_publisher_assert_liveliness(&publisher)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +// Mocking rmw_publisher_wait_for_all_acked to make +// rcl_publisher_wait_for_all_acked fail +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rmw_time_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rmw_time_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rmw_time_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rmw_time_t, >) + +TEST_F( + CLASSNAME(TestPublisherFixtureInit, RMW_IMPLEMENTATION), + test_mock_assert_wait_for_all_acked) +{ +#define CHECK_PUBLISHER_WAIT_FOR_ALL_ACKED_RETURN(RMW_RET_RESULT, EXPECT_RET) do { \ + rmw_publisher_wait_for_all_acked_return = RMW_RET_RESULT; \ + ret = rcl_publisher_wait_for_all_acked(&publisher, 1000000); \ + EXPECT_EQ(EXPECT_RET, ret); \ + rcl_reset_error(); \ +} while (0) + + rcl_ret_t ret; + rmw_ret_t rmw_publisher_wait_for_all_acked_return; + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_publisher_wait_for_all_acked, rmw_publisher_wait_for_all_acked_return); + + { + // Now normal usage of the function rcl_publisher_wait_for_all_acked returning + // unexpected RMW_RET_TIMEOUT + SCOPED_TRACE("Check RCL return failed !"); + CHECK_PUBLISHER_WAIT_FOR_ALL_ACKED_RETURN(RMW_RET_TIMEOUT, RCL_RET_TIMEOUT); + } + + { + // Now normal usage of the function rcl_publisher_wait_for_all_acked returning + // unexpected RMW_RET_UNSUPPORTED + SCOPED_TRACE("Check RCL return failed !"); + CHECK_PUBLISHER_WAIT_FOR_ALL_ACKED_RETURN(RMW_RET_UNSUPPORTED, RCL_RET_UNSUPPORTED); + } + + { + // Now normal usage of the function rcl_publisher_wait_for_all_acked returning + // unexpected RMW_RET_INVALID_ARGUMENT + SCOPED_TRACE("Check RCL return failed !"); + CHECK_PUBLISHER_WAIT_FOR_ALL_ACKED_RETURN(RMW_RET_INVALID_ARGUMENT, RCL_RET_ERROR); + } + + { + // Now normal usage of the function rcl_publisher_wait_for_all_acked returning + // unexpected RMW_RET_INCORRECT_RMW_IMPLEMENTATION + SCOPED_TRACE("Check RCL return failed !"); + CHECK_PUBLISHER_WAIT_FOR_ALL_ACKED_RETURN(RMW_RET_INCORRECT_RMW_IMPLEMENTATION, RCL_RET_ERROR); + } + + { + // Now normal usage of the function rcl_publisher_wait_for_all_acked returning + // unexpected RMW_RET_ERROR + SCOPED_TRACE("Check RCL return failed !"); + CHECK_PUBLISHER_WAIT_FOR_ALL_ACKED_RETURN(RMW_RET_ERROR, RCL_RET_ERROR); + } +} + +// Mocking rmw_publish to make rcl_publish fail +TEST_F(CLASSNAME(TestPublisherFixtureInit, RMW_IMPLEMENTATION), test_mock_publish) { + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_publish, RMW_RET_ERROR); + + // Test normal usage of the function rcl_publish returning unexpected RMW_RET_ERROR + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + msg.int64_value = 42; + rcl_ret_t ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__BasicTypes__fini(&msg); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +// Mocking rmw_publish_serialized_message to make rcl_publish_serialized_message fail +TEST_F( + CLASSNAME(TestPublisherFixtureInit, RMW_IMPLEMENTATION), test_mock_publish_serialized_message) +{ + rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); + size_t initial_size_serialized = 0u; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ASSERT_EQ( + RCL_RET_OK, rmw_serialized_message_init( + &serialized_msg, initial_size_serialized, &allocator)) << rcl_get_error_string().str; + constexpr char test_string[] = "testing"; + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ( + RMW_RET_OK, + rmw_serialized_message_fini(&serialized_msg)) << rcl_get_error_string().str; + }); + + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ASSERT_STREQ(msg.string_value.data, test_string); + rcl_ret_t ret = rmw_serialize(&msg, ts, &serialized_msg); + ASSERT_EQ(RMW_RET_OK, ret); + + rmw_ret_t rmw_publish_serialized_return = RMW_RET_ERROR; + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_publish_serialized_message, rmw_publish_serialized_return); + { + // Test normal usage of the function rcl_publish_serialized_message + // returning unexpected RMW_RET_ERROR + ret = rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + // Repeat, but now returning BAD_ALLOC + rmw_publish_serialized_return = RMW_RET_BAD_ALLOC; + ret = rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } +} + +// Define dummy comparison operators for rcutils_allocator_t type for use with the Mimick Library +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) + +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_mock_publisher_init) { + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic_name[] = "chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = RCL_RET_OK; + + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_string_map_init, RCUTILS_RET_ERROR); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST_F( + CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_mock_publisher_init_fail_qos) +{ + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_publisher_get_actual_qos, RMW_RET_ERROR); + + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic_name[] = "chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + + rcl_ret_t ret = + rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +// Tests for loaned msgs functions. Mocked as the rmw tier1 vendors don't support it +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_mock_loaned_functions) { + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + rcl_publisher_t not_init_publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "chatter"; + constexpr char expected_topic_name[] = "/chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + + rcl_ret_t ret = rcl_publisher_init( + &publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + EXPECT_EQ(strcmp(rcl_publisher_get_topic_name(&publisher), expected_topic_name), 0); + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + msg.int64_value = 42; + void * msg_pointer = &msg; + rmw_publisher_allocation_t * null_allocation_is_valid_arg = nullptr; + + { + // mocked, publish nominal usage + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_publish_loaned_message, RMW_RET_OK); + EXPECT_EQ(RCL_RET_OK, rcl_publish_loaned_message(&publisher, &msg, nullptr)); + } + { + // bad params publish + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_publish_loaned_message(nullptr, &msg, null_allocation_is_valid_arg)); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_publish_loaned_message(¬_init_publisher, &msg, null_allocation_is_valid_arg)); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_publish_loaned_message(&publisher, nullptr, null_allocation_is_valid_arg)); + } + { + // mocked, failure publish + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_publish_loaned_message, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_publish_loaned_message(&publisher, &msg, nullptr)); + } + { + // mocked, borrow loaned nominal usage + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_borrow_loaned_message, RMW_RET_OK); + EXPECT_EQ(RCL_RET_OK, rcl_borrow_loaned_message(&publisher, ts, &msg_pointer)); + } + { + // bad params borrow loaned + EXPECT_EQ(RCL_RET_PUBLISHER_INVALID, rcl_borrow_loaned_message(nullptr, ts, &msg_pointer)); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, rcl_borrow_loaned_message(¬_init_publisher, ts, &msg_pointer)); + } + { + // mocked, nominal return loaned message + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_return_loaned_message_from_publisher, RMW_RET_OK); + EXPECT_EQ(RCL_RET_OK, rcl_return_loaned_message_from_publisher(&publisher, &msg)); + } + { + // bad params return loaned message + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_return_loaned_message_from_publisher(nullptr, &msg)); + EXPECT_EQ( + RCL_RET_PUBLISHER_INVALID, + rcl_return_loaned_message_from_publisher(¬_init_publisher, &msg)); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_return_loaned_message_from_publisher(&publisher, nullptr)); + } + + test_msgs__msg__BasicTypes__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +// Tests mocking ini/fini functions for specific failures +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_mocks_fail_publisher_init) { + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic_name[] = "chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = RCL_RET_OK; + + { + // Internal rmw failure validating node name + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_validate_node_name, RMW_RET_ERROR); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + { + // Internal rmw failure validating node name + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_validate_node_name, RMW_RET_INVALID_ARGUMENT); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + { + // Internal failure when fini rcutils_string_map returns error, targets substitution_map fini + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_string_map_fini, RCUTILS_RET_ERROR); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + { + // Internal rmw failure validating topic name + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_validate_full_topic_name, RMW_RET_ERROR); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } + { + // Internal rmw failure validating node name, returns OK but the result is set to error + auto mock = mocking_utils::patch( + "lib:rcl", rmw_validate_full_topic_name, [](auto, int * result, auto) { + *result = RMW_TOPIC_INVALID_NOT_ABSOLUTE; + return RMW_RET_OK; + }); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + EXPECT_EQ(RCL_RET_TOPIC_NAME_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + } +} + +// Test mocked fail fini publisher +TEST_F(CLASSNAME(TestPublisherFixture, RMW_IMPLEMENTATION), test_mock_publisher_fini_fail) { + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_ret_t ret = rcl_publisher_init( + &publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Internal rmw failure destroying publisher + auto mock = mocking_utils::patch_and_return("lib:rcl", rmw_destroy_publisher, RMW_RET_ERROR); + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; +} diff --git a/test/rcl/test_publisher_wait_all_ack.cpp b/test/rcl/test_publisher_wait_all_ack.cpp new file mode 100644 index 0000000..c931c09 --- /dev/null +++ b/test/rcl/test_publisher_wait_all_ack.cpp @@ -0,0 +1,208 @@ +// 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 +#include +#include + +#include "rcl/allocator.h" +#include "rcl/publisher.h" +#include "rcl/subscription.h" +#include "rcpputils/filesystem_helper.hpp" +#include "rcutils/env.h" + +#include "rcl/rcl.h" +#include "test_msgs/msg/strings.h" +#include "test_msgs/msg/basic_types.h" +#include "rosidl_runtime_c/string_functions.h" +#include "wait_for_entity_helpers.hpp" + +#include "mimick/mimick.h" +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + + +/* This class is used for test_wait_for_all_acked + */ +class CLASSNAME (TestPublisherFixtureSpecial, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + + void SetUp() + { + bool is_fastdds = (std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps") == 0); + + if (is_fastdds) { + // By default, fastdds use intraprocess mode in this scenario. But this leads to high-speed + // data transmission. test_wait_for_all_acked need low data transmission. So disable this + // mode via fastdds profile file. + rcpputils::fs::path fastdds_profile(TEST_RESOURCES_DIRECTORY); + fastdds_profile /= "test_profile/disable_intraprocess.xml"; + ASSERT_EQ( + rcutils_set_env("FASTRTPS_DEFAULT_PROFILES_FILE", fastdds_profile.string().c_str()), + true); + } + + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + constexpr char name[] = "test_publisher_node2"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcutils_set_env("FASTRTPS_DEFAULT_PROFILES_FILE", NULL); + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +#define INIT_SUBSCRIPTION(idx) \ + rcl_subscription_t subscription ## idx = rcl_get_zero_initialized_subscription(); \ + ret = rcl_subscription_init( \ + &subscription ## idx, \ + this->node_ptr, \ + ts, \ + topic_name, \ + &subscription_options); \ + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( \ + { \ + ret = rcl_subscription_fini(&subscription ## idx, this->node_ptr); \ + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ + }); + +#define ONE_MEGABYTE (1024 * 1024) + +TEST_F(CLASSNAME(TestPublisherFixtureSpecial, RMW_IMPLEMENTATION), test_wait_for_all_acked) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "test_wait_for_all_acked"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + publisher_options.qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + publisher_options.qos.depth = 10000; + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + if (ret != RCL_RET_OK) { + FAIL() << rcl_get_error_string().str; + } + }); + + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + subscription_options.qos.depth = 1; + subscription_options.qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + + INIT_SUBSCRIPTION(1) + INIT_SUBSCRIPTION(2) + INIT_SUBSCRIPTION(3) + + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + char * test_string = static_cast(allocator.allocate(ONE_MEGABYTE, allocator.state)); + ASSERT_TRUE(test_string != NULL); + memset(test_string, 'a', ONE_MEGABYTE); + test_string[ONE_MEGABYTE - 1] = '\0'; + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + allocator.deallocate(test_string, allocator.state); + test_msgs__msg__Strings__fini(&msg); + }); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + + ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription1, context_ptr, 10, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription2, context_ptr, 10, 100)); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription3, context_ptr, 10, 100)); + + int i = 0; + for (; i < 500; i++) { + ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + ret = rcl_publisher_wait_for_all_acked( + &publisher, + RCL_MS_TO_NS(500)); + EXPECT_TRUE(ret == RCL_RET_OK || ret == RCL_RET_TIMEOUT); + + ret = rcl_publisher_wait_for_all_acked(&publisher, -1); + EXPECT_EQ(RCL_RET_OK, ret); +} + +TEST_F( + CLASSNAME(TestPublisherFixtureSpecial, RMW_IMPLEMENTATION), + test_wait_for_all_acked_with_best_effort) +{ + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic_name[] = "test_wait_for_all_acked_with_best_effort"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + publisher_options.qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + publisher_options.qos.depth = 10000; + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic_name, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_publisher_wait_for_all_acked( + &publisher, + RCL_MS_TO_NS(500)); + EXPECT_EQ(RCL_RET_OK, ret); + + ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} diff --git a/test/rcl/test_remap.cpp b/test/rcl/test_remap.cpp new file mode 100644 index 0000000..b8ae663 --- /dev/null +++ b/test/rcl/test_remap.cpp @@ -0,0 +1,647 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/arguments.h" +#include "rcl/rcl.h" +#include "rcl/remap.h" +#include "rcl/error_handling.h" + +#include "./allocator_testing_utils.h" +#include "./arg_macros.hpp" +#include "./arguments_impl.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestRemapFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + void SetUp() + { + } + + void TearDown() + { + } +}; + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), global_namespace_replacement) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "__ns:=/foo/bar"); + + char * output = NULL; + ret = rcl_remap_node_namespace( + NULL, &global_arguments, "NodeName", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/foo/bar", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), nodename_prefix_namespace_remap) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS( + global_arguments, + "process_name", + "--ros-args", + "-r", "Node1:__ns:=/foo/bar", + "-r", "Node2:__ns:=/this_one", + "-r", "Node3:__ns:=/bar/foo"); + + { + char * output = NULL; + ret = rcl_remap_node_namespace( + NULL, &global_arguments, "Node1", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/foo/bar", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); + } + { + char * output = NULL; + ret = rcl_remap_node_namespace( + NULL, &global_arguments, "Node2", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/this_one", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); + } + { + char * output = NULL; + ret = rcl_remap_node_namespace( + NULL, &global_arguments, "Node3", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/bar/foo", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); + } +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_namespace_replacement) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name"); + + char * output = NULL; + ret = rcl_remap_node_namespace( + NULL, &global_arguments, "NodeName", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(NULL, output); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), local_namespace_replacement_before_global) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "__ns:=/global_args"); + rcl_arguments_t local_arguments; + SCOPE_ARGS(local_arguments, "process_name", "--ros-args", "-r", "__ns:=/local_args"); + + char * output = NULL; + ret = rcl_remap_node_namespace( + &local_arguments, &global_arguments, "NodeName", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/local_args", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_use_global_namespace_replacement) { + rcl_ret_t ret; + rcl_arguments_t local_arguments; + SCOPE_ARGS(local_arguments, "process_name"); + + char * output = NULL; + ret = rcl_remap_node_namespace( + &local_arguments, NULL, "NodeName", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(NULL, output); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), other_rules_before_namespace_rule) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS( + global_arguments, + "process_name", + "--ros-args", + "-r", "/foobar:=/foo/bar", + "-r", "__ns:=/namespace", + "-r", "__node:=new_name"); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + char * output = NULL; + ret = rcl_remap_node_namespace(NULL, &global_arguments, "NodeName", allocator, &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/namespace", output); + allocator.deallocate(output, allocator.state); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), global_topic_name_replacement) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "/bar/foo:=/foo/bar"); + + { + char * output = NULL; + ret = rcl_remap_topic_name( + NULL, &global_arguments, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_STREQ("/foo/bar", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); + } + { + char * output = NULL; + ret = rcl_remap_topic_name( + NULL, &global_arguments, "/foo/bar", "NodeName", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(NULL, output); + } +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), topic_and_service_name_not_null) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "/bar/foo:=/foo/bar"); + + { + char * output = NULL; + ret = rcl_remap_service_name( + NULL, &global_arguments, NULL, "NodeName", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + ASSERT_EQ(NULL, output); + } + { + char * output = NULL; + ret = rcl_remap_topic_name( + NULL, &global_arguments, NULL, "NodeName", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_EQ(NULL, output); + } +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), relative_topic_name_remap) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "foo:=bar"); + + char * output = NULL; + ret = rcl_remap_topic_name( + NULL, &global_arguments, "/ns/foo", "NodeName", "/ns", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_STREQ("/ns/bar", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), nodename_prefix_topic_remap) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS( + global_arguments, + "process_name", + "--ros-args", + "-r", "Node1:/foo:=/foo/bar", + "-r", "Node2:/foo:=/this_one", + "-r", "Node3:/foo:=/bar/foo"); + + { + char * output = NULL; + ret = rcl_remap_topic_name( + NULL, &global_arguments, "/foo", "Node1", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/foo/bar", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); + } + { + char * output = NULL; + ret = rcl_remap_topic_name( + NULL, &global_arguments, "/foo", "Node2", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/this_one", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); + } + { + char * output = NULL; + ret = rcl_remap_topic_name( + NULL, &global_arguments, "/foo", "Node3", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/bar/foo", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); + } +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_use_global_topic_name_replacement) { + rcl_ret_t ret; + rcl_arguments_t local_arguments; + SCOPE_ARGS(local_arguments, "process_name"); + + char * output = NULL; + ret = rcl_remap_topic_name( + &local_arguments, NULL, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(NULL, output); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_topic_name_replacement) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name"); + + char * output = NULL; + ret = rcl_remap_topic_name( + NULL, &global_arguments, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(NULL, output); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), local_topic_replacement_before_global) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "/bar/foo:=/foo/global_args"); + rcl_arguments_t local_arguments; + SCOPE_ARGS(local_arguments, "process_name", "--ros-args", "-r", "/bar/foo:=/foo/local_args"); + + char * output = NULL; + ret = rcl_remap_topic_name( + &local_arguments, &global_arguments, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(), + &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/foo/local_args", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), other_rules_before_topic_rule) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS( + global_arguments, + "process_name", + "--ros-args", + "-r", "__ns:=/namespace", + "-r", "__node:=remap_name", + "-r", "/foobar:=/foo/bar"); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + char * output = NULL; + ret = rcl_remap_topic_name( + NULL, &global_arguments, "/foobar", "NodeName", "/", allocator, &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/foo/bar", output); + allocator.deallocate(output, allocator.state); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), global_service_name_replacement) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "/bar/foo:=/foo/bar"); + + { + char * output = NULL; + ret = rcl_remap_service_name( + NULL, &global_arguments, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_STREQ("/foo/bar", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); + } + { + char * output = NULL; + ret = rcl_remap_service_name( + NULL, &global_arguments, "/foobar", "NodeName", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(NULL, output); + } +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), relative_service_name_remap) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "foo:=bar"); + + char * output = NULL; + ret = rcl_remap_service_name( + NULL, &global_arguments, "/ns/foo", "NodeName", "/ns", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_STREQ("/ns/bar", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), nodename_prefix_service_remap) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS( + global_arguments, + "process_name", + "--ros-args", + "-r", "Node1:/foo:=/foo/bar", + "-r", "Node2:/foo:=/this_one", + "-r", "Node3:/foo:=/bar/foo"); + + { + char * output = NULL; + ret = rcl_remap_service_name( + NULL, &global_arguments, "/foo", "Node1", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/foo/bar", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); + } + { + char * output = NULL; + ret = rcl_remap_service_name( + NULL, &global_arguments, "/foo", "Node2", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/this_one", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); + } + { + char * output = NULL; + ret = rcl_remap_service_name( + NULL, &global_arguments, "/foo", "Node3", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/bar/foo", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); + } +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_use_global_service_name_replacement) { + rcl_ret_t ret; + rcl_arguments_t local_arguments; + SCOPE_ARGS(local_arguments, "process_name"); + + char * output = NULL; + ret = rcl_remap_service_name( + &local_arguments, NULL, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(), + &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(NULL, output); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_service_name_replacement) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name"); + + char * output = NULL; + ret = rcl_remap_service_name( + NULL, &global_arguments, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(NULL, output); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), local_service_replacement_before_global) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "/bar/foo:=/foo/global_args"); + rcl_arguments_t local_arguments; + SCOPE_ARGS(local_arguments, "process_name", "--ros-args", "-r", "/bar/foo:=/foo/local_args"); + + char * output = NULL; + ret = rcl_remap_service_name( + &local_arguments, &global_arguments, "/bar/foo", "NodeName", "/", rcl_get_default_allocator(), + &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/foo/local_args", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), other_rules_before_service_rule) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS( + global_arguments, + "process_name", + "--ros-args", + "-r", "__ns:=/namespace", + "-r", "__node:=remap_name", + "-r", "/foobar:=/foo/bar"); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + char * output = NULL; + ret = rcl_remap_service_name( + NULL, &global_arguments, "/foobar", "NodeName", "/", allocator, &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("/foo/bar", output); + allocator.deallocate(output, allocator.state); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), global_nodename_replacement) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "__node:=globalname"); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + char * output = NULL; + ret = rcl_remap_node_name(NULL, &global_arguments, "NodeName", allocator, &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("globalname", output); + allocator.deallocate(output, allocator.state); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_nodename_replacement) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name"); + + char * output = NULL; + ret = rcl_remap_node_name( + NULL, &global_arguments, "NodeName", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(NULL, output); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), local_nodename_replacement_before_global) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "__node:=global_name"); + rcl_arguments_t local_arguments; + SCOPE_ARGS(local_arguments, "process_name", "--ros-args", "-r", "__node:=local_name"); + + char * output = NULL; + ret = rcl_remap_node_name( + &local_arguments, &global_arguments, "NodeName", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("local_name", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), no_use_global_nodename_replacement) { + rcl_ret_t ret; + rcl_arguments_t local_arguments; + SCOPE_ARGS(local_arguments, "process_name"); + + char * output = NULL; + ret = rcl_remap_node_name( + &local_arguments, NULL, "NodeName", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(NULL, output); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), use_first_nodename_rule) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS( + global_arguments, + "process_name", + "--ros-args", + "-r", "__node:=firstname", + "-r", "__node:=secondname"); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + char * output = NULL; + ret = rcl_remap_node_name(NULL, &global_arguments, "NodeName", allocator, &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("firstname", output); + allocator.deallocate(output, allocator.state); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), other_rules_before_nodename_rule) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS( + global_arguments, + "process_name", + "--ros-args", + "-r", "/foobar:=/foo", + "-r", "__ns:=/namespace", + "-r", "__node:=remap_name"); + + rcl_allocator_t allocator = rcl_get_default_allocator(); + char * output = NULL; + ret = rcl_remap_node_name(NULL, &global_arguments, "NodeName", allocator, &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("remap_name", output); + allocator.deallocate(output, allocator.state); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), url_scheme_rosservice) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "rosservice://foo:=bar"); + + char * output = NULL; + ret = rcl_remap_service_name( + NULL, &global_arguments, "/ns/foo", "NodeName", "/ns", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_STREQ("/ns/bar", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); + + ret = rcl_remap_topic_name( + NULL, &global_arguments, "/ns/foo", "NodeName", "/ns", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(NULL, output); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), url_scheme_rostopic) { + rcl_ret_t ret; + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "rostopic://foo:=bar"); + + char * output = NULL; + ret = rcl_remap_topic_name( + NULL, &global_arguments, "/ns/foo", "NodeName", "/ns", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + ASSERT_STREQ("/ns/bar", output); + rcl_get_default_allocator().deallocate(output, rcl_get_default_allocator().state); + + ret = rcl_remap_service_name( + NULL, &global_arguments, "/ns/foo", "NodeName", "/ns", rcl_get_default_allocator(), &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_EQ(NULL, output); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), _rcl_remap_name_bad_arg) { + rcl_arguments_t global_arguments; + SCOPE_ARGS(global_arguments, "process_name", "--ros-args", "-r", "__node:=global_name"); + rcl_arguments_t local_arguments; + SCOPE_ARGS(local_arguments, "process_name", "--ros-args", "-r", "__node:=local_name"); + rcl_arguments_t zero_init_global_arguments = rcl_get_zero_initialized_arguments(); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t bad_allocator = get_failing_allocator(); + char * output = NULL; + + // Expected usage local_args, global not init is OK + rcl_ret_t ret = rcl_remap_node_name( + &local_arguments, &zero_init_global_arguments, "NodeName", allocator, &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("local_name", output); + allocator.deallocate(output, allocator.state); + + // Expected usage global_args, local not null is OK + ret = rcl_remap_node_name(nullptr, &global_arguments, "NodeName", allocator, &output); + EXPECT_EQ(RCL_RET_OK, ret); + EXPECT_STREQ("global_name", output); + allocator.deallocate(output, allocator.state); + + // Both local and global arguments, not valid + ret = rcl_remap_node_name(nullptr, nullptr, "NodeName", allocator, &output); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + // Bad allocator + ret = rcl_remap_node_name(nullptr, &global_arguments, "NodeName", bad_allocator, &output); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); +} + +TEST_F(CLASSNAME(TestRemapFixture, RMW_IMPLEMENTATION), internal_remap_use) { + // Easiest way to init a rcl_remap is through the arguments API + const char * argv[] = { + "process_name", "--ros-args", "-r", "__ns:=/namespace", "random:=arg" + }; + int argc = sizeof(argv) / sizeof(const char *); + rcl_allocator_t alloc = rcl_get_default_allocator(); + rcl_arguments_t parsed_args = rcl_get_zero_initialized_arguments(); + + rcl_ret_t ret = rcl_parse_arguments(argc, argv, alloc, &parsed_args); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_arguments_fini(&parsed_args)); + }); + + // Bad alloc + rcl_remap_t remap_dst = rcl_get_zero_initialized_remap(); + parsed_args.impl->remap_rules->impl->allocator = get_failing_allocator(); + EXPECT_EQ(RCL_RET_BAD_ALLOC, rcl_remap_copy(parsed_args.impl->remap_rules, &remap_dst)); + parsed_args.impl->remap_rules->impl->allocator = alloc; + + // Not valid null ptrs + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_remap_copy(nullptr, &remap_dst)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_remap_copy(parsed_args.impl->remap_rules, nullptr)); + rcl_reset_error(); + + // Not valid empty source + rcl_remap_t remap_empty = rcl_get_zero_initialized_remap(); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_remap_copy(&remap_empty, &remap_dst)); + rcl_reset_error(); + + // Expected usage + EXPECT_EQ(RCL_RET_OK, rcl_remap_copy(parsed_args.impl->remap_rules, &remap_dst)); + + // Copy twice + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_remap_copy(parsed_args.impl->remap_rules, &remap_dst)); + rcl_reset_error(); + + // Fini + EXPECT_EQ(RCL_RET_OK, rcl_remap_fini(&remap_dst)); + + // Fini twice + EXPECT_EQ(RCL_RET_ERROR, rcl_remap_fini(&remap_dst)); + rcl_reset_error(); + + // Bad fini + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_remap_fini(nullptr)); +} diff --git a/test/rcl/test_remap_integration.cpp b/test/rcl/test_remap_integration.cpp new file mode 100644 index 0000000..60ff519 --- /dev/null +++ b/test/rcl/test_remap_integration.cpp @@ -0,0 +1,335 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "rcl/rcl.h" +#include "rcl/remap.h" +#include "rcl/error_handling.h" + +#include "test_msgs/msg/basic_types.h" +#include "test_msgs/srv/basic_types.h" + +#include "./arg_macros.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestRemapIntegrationFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + void SetUp() + { + } + + void TearDown() + { + } +}; + +TEST_F(CLASSNAME(TestRemapIntegrationFixture, RMW_IMPLEMENTATION), remap_using_global_rule) { + int argc; + char ** argv; + SCOPE_GLOBAL_ARGS( + argc, argv, + "process_name", + "--ros-args", + "-r", "__node:=new_name", + "-r", "__ns:=/new_ns", + "-r", "/foo/bar:=/bar/foo"); + + rcl_node_t node = rcl_get_zero_initialized_node(); + rcl_node_options_t default_options = rcl_node_get_default_options(); + ASSERT_EQ( + RCL_RET_OK, + rcl_node_init(&node, "original_name", "/original_ns", &context, &default_options)); + + { // Node name gets remapped + EXPECT_STREQ("new_name", rcl_node_get_name(&node)); + } + { // Node namespace gets remapped + EXPECT_STREQ("/new_ns", rcl_node_get_namespace(&node)); + } + { // Logger name gets remapped + EXPECT_STREQ("new_ns.new_name", rcl_node_get_logger_name(&node)); + } + { // Publisher topic gets remapped + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + rcl_ret_t ret = rcl_publisher_init(&publisher, &node, ts, "/foo/bar", &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/bar/foo", rcl_publisher_get_topic_name(&publisher)); + EXPECT_EQ(RCL_RET_OK, rcl_publisher_fini(&publisher, &node)); + } + { // Subscription topic gets remapped + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_ret_t ret = rcl_subscription_init( + &subscription, &node, ts, "/foo/bar", &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/bar/foo", rcl_subscription_get_topic_name(&subscription)); + EXPECT_EQ(RCL_RET_OK, rcl_subscription_fini(&subscription, &node)); + } + { // Client service name gets remapped + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + rcl_client_options_t client_options = rcl_client_get_default_options(); + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_ret_t ret = rcl_client_init(&client, &node, ts, "/foo/bar", &client_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/bar/foo", rcl_client_get_service_name(&client)); + EXPECT_EQ(RCL_RET_OK, rcl_client_fini(&client, &node)); + } + { // Server service name gets remapped + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + rcl_service_options_t service_options = rcl_service_get_default_options(); + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_ret_t ret = rcl_service_init(&service, &node, ts, "/foo/bar", &service_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/bar/foo", rcl_service_get_service_name(&service)); + EXPECT_EQ(RCL_RET_OK, rcl_service_fini(&service, &node)); + } + + EXPECT_EQ(RCL_RET_OK, rcl_node_fini(&node)); +} + +TEST_F(CLASSNAME(TestRemapIntegrationFixture, RMW_IMPLEMENTATION), ignore_global_rules) { + int argc; + char ** argv; + SCOPE_GLOBAL_ARGS( + argc, argv, + "process_name", + "--ros-args", + "-r", "__node:=new_name", + "-r", "__ns:=/new_ns", + "-r", "/foo/bar:=/bar/foo"); + rcl_arguments_t local_arguments; + SCOPE_ARGS(local_arguments, "local_process_name"); + + rcl_node_t node = rcl_get_zero_initialized_node(); + rcl_node_options_t options = rcl_node_get_default_options(); + options.use_global_arguments = false; + options.arguments = local_arguments; + ASSERT_EQ(RCL_RET_OK, rcl_node_init(&node, "original_name", "/original_ns", &context, &options)); + + { // Node name does not get remapped + EXPECT_STREQ("original_name", rcl_node_get_name(&node)); + } + { // Node namespace does not get remapped + EXPECT_STREQ("/original_ns", rcl_node_get_namespace(&node)); + } + { // Logger name gets remapped + EXPECT_STREQ("original_ns.original_name", rcl_node_get_logger_name(&node)); + } + { // Publisher topic does not get remapped + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + rcl_ret_t ret = rcl_publisher_init(&publisher, &node, ts, "/foo/bar", &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/foo/bar", rcl_publisher_get_topic_name(&publisher)); + EXPECT_EQ(RCL_RET_OK, rcl_publisher_fini(&publisher, &node)); + } + { // Subscription topic does not get remapped + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_ret_t ret = rcl_subscription_init( + &subscription, &node, ts, "/foo/bar", &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/foo/bar", rcl_subscription_get_topic_name(&subscription)); + EXPECT_EQ(RCL_RET_OK, rcl_subscription_fini(&subscription, &node)); + } + { // Client service name does not get remapped + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + rcl_client_options_t client_options = rcl_client_get_default_options(); + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_ret_t ret = rcl_client_init(&client, &node, ts, "/foo/bar", &client_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/foo/bar", rcl_client_get_service_name(&client)); + EXPECT_EQ(RCL_RET_OK, rcl_client_fini(&client, &node)); + } + { // Server service name does not get remapped + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + rcl_service_options_t service_options = rcl_service_get_default_options(); + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_ret_t ret = rcl_service_init(&service, &node, ts, "/foo/bar", &service_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/foo/bar", rcl_service_get_service_name(&service)); + EXPECT_EQ(RCL_RET_OK, rcl_service_fini(&service, &node)); + } + + EXPECT_EQ(RCL_RET_OK, rcl_node_fini(&node)); +} + +TEST_F(CLASSNAME(TestRemapIntegrationFixture, RMW_IMPLEMENTATION), local_rules_before_global) { + int argc; + char ** argv; + SCOPE_GLOBAL_ARGS( + argc, argv, + "process_name", + "--ros-args", + "-r", "__node:=global_name", + "-r", "__ns:=/global_ns", + "-r", "/foo/bar:=/bar/global"); + rcl_arguments_t local_arguments; + SCOPE_ARGS( + local_arguments, + "process_name", + "--ros-args", + "-r", "__node:=local_name", + "-r", "__ns:=/local_ns", + "-r", "/foo/bar:=/bar/local"); + + rcl_node_t node = rcl_get_zero_initialized_node(); + rcl_node_options_t options = rcl_node_get_default_options(); + options.arguments = local_arguments; + ASSERT_EQ(RCL_RET_OK, rcl_node_init(&node, "original_name", "/original_ns", &context, &options)); + + { // Node name + EXPECT_STREQ("local_name", rcl_node_get_name(&node)); + } + { // Node namespace + EXPECT_STREQ("/local_ns", rcl_node_get_namespace(&node)); + } + { // Logger name + EXPECT_STREQ("local_ns.local_name", rcl_node_get_logger_name(&node)); + } + { // Publisher topic + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + rcl_ret_t ret = rcl_publisher_init(&publisher, &node, ts, "/foo/bar", &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/bar/local", rcl_publisher_get_topic_name(&publisher)); + EXPECT_EQ(RCL_RET_OK, rcl_publisher_fini(&publisher, &node)); + } + { // Subscription topic + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_ret_t ret = rcl_subscription_init( + &subscription, &node, ts, "/foo/bar", &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/bar/local", rcl_subscription_get_topic_name(&subscription)); + EXPECT_EQ(RCL_RET_OK, rcl_subscription_fini(&subscription, &node)); + } + { // Client service name + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + rcl_client_options_t client_options = rcl_client_get_default_options(); + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_ret_t ret = rcl_client_init(&client, &node, ts, "/foo/bar", &client_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/bar/local", rcl_client_get_service_name(&client)); + EXPECT_EQ(RCL_RET_OK, rcl_client_fini(&client, &node)); + } + { // Server service name + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + rcl_service_options_t service_options = rcl_service_get_default_options(); + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_ret_t ret = rcl_service_init(&service, &node, ts, "/foo/bar", &service_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/bar/local", rcl_service_get_service_name(&service)); + EXPECT_EQ(RCL_RET_OK, rcl_service_fini(&service, &node)); + } + + EXPECT_EQ(RCL_RET_OK, rcl_node_fini(&node)); +} + +TEST_F(CLASSNAME(TestRemapIntegrationFixture, RMW_IMPLEMENTATION), remap_relative_topic) { + int argc; + char ** argv; + SCOPE_GLOBAL_ARGS(argc, argv, "process_name", "--ros-args", "-r", "/foo/bar:=remap/global"); + + rcl_node_t node = rcl_get_zero_initialized_node(); + rcl_node_options_t default_options = rcl_node_get_default_options(); + ASSERT_EQ(RCL_RET_OK, rcl_node_init(&node, "original_name", "/foo", &context, &default_options)); + + { // Publisher topic + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + rcl_ret_t ret = rcl_publisher_init(&publisher, &node, ts, "bar", &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/foo/remap/global", rcl_publisher_get_topic_name(&publisher)); + EXPECT_EQ(RCL_RET_OK, rcl_publisher_fini(&publisher, &node)); + } + { // Subscription topic + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_ret_t ret = rcl_subscription_init( + &subscription, &node, ts, "bar", &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/foo/remap/global", rcl_subscription_get_topic_name(&subscription)); + EXPECT_EQ(RCL_RET_OK, rcl_subscription_fini(&subscription, &node)); + } + { // Client service name + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + rcl_client_options_t client_options = rcl_client_get_default_options(); + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_ret_t ret = rcl_client_init(&client, &node, ts, "bar", &client_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/foo/remap/global", rcl_client_get_service_name(&client)); + EXPECT_EQ(RCL_RET_OK, rcl_client_fini(&client, &node)); + } + { // Server service name + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + rcl_service_options_t service_options = rcl_service_get_default_options(); + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_ret_t ret = rcl_service_init(&service, &node, ts, "bar", &service_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_STREQ("/foo/remap/global", rcl_service_get_service_name(&service)); + EXPECT_EQ(RCL_RET_OK, rcl_service_fini(&service, &node)); + } + + EXPECT_EQ(RCL_RET_OK, rcl_node_fini(&node)); +} + +TEST_F(CLASSNAME(TestRemapIntegrationFixture, RMW_IMPLEMENTATION), remap_using_node_rules) { + int argc; + char ** argv; + SCOPE_GLOBAL_ARGS( + argc, argv, "process_name", "--ros-args", "-r", "original_name:__ns:=/new_ns"); + + rcl_node_t node = rcl_get_zero_initialized_node(); + rcl_node_options_t default_options = rcl_node_get_default_options(); + ASSERT_EQ(RCL_RET_OK, rcl_node_init(&node, "original_name", "", &context, &default_options)); + + { // Node namespace gets remapped + EXPECT_STREQ("/new_ns", rcl_node_get_namespace(&node)); + } + EXPECT_EQ(RCL_RET_OK, rcl_node_fini(&node)); +} diff --git a/test/rcl/test_rmw_impl_id_check.py.in b/test/rcl/test_rmw_impl_id_check.py.in new file mode 100644 index 0000000..c970687 --- /dev/null +++ b/test/rcl/test_rmw_impl_id_check.py.in @@ -0,0 +1,80 @@ +# generated from rcl/test/test_rmw_impl_id_check.py.in + +import os + +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch.actions import OpaqueFunction + +import launch_testing +import launch_testing.actions +import launch_testing.asserts +import launch_testing.util + +import unittest + +@launch_testing.parametrize( + 'rmw_implementation_env,rcl_assert_rmw_id_matches_env', + [ + # Test RMW_IMPLEMENTATION only. + ('@rmw_implementation@', None), + ('garbage', None), + ('', None), + # Test RCL_ASSERT_RMW_ID_MATCHES only. + (None, 'garbage'), + (None, ''), + # Test both. + ('@rmw_implementation@', '@rmw_implementation@'), + ('garbage', '@rmw_implementation@'), + ('@rmw_implementation@', 'garbage'), + ('garbage', 'garbage'), + ('', ''), + ] +) +def generate_test_description( + rmw_implementation_env, + rcl_assert_rmw_id_matches_env, +): + launch_description = LaunchDescription() + + env = dict(os.environ) + if rmw_implementation_env is not None: + env['RMW_IMPLEMENTATION'] = rmw_implementation_env + if rcl_assert_rmw_id_matches_env is not None: + env['RCL_ASSERT_RMW_ID_MATCHES'] = rcl_assert_rmw_id_matches_env + + executable_under_test = ExecuteProcess( + cmd=['@TEST_RMW_IMPL_ID_CHECK_EXECUTABLE_NAME@'], + name='@TEST_RMW_IMPL_ID_CHECK_EXECUTABLE_NAME@', + env=env, + ) + launch_description.add_action(executable_under_test) + # Keep the test fixture alive till tests end. + launch_description.add_action(launch_testing.util.KeepAliveProc()) + launch_description.add_action( + launch_testing.actions.ReadyToTest() + ) + return launch_description, locals() + +class TestRMWImplementationIDCheck(unittest.TestCase): + + def test_process_terminates_in_a_finite_amount_of_time(self, executable_under_test, proc_info): + """Test that executable under test terminates in a finite amount of time.""" + proc_info.assertWaitForShutdown(process=executable_under_test, timeout=10) + +@launch_testing.post_shutdown_test() +class TestRMWImplementationIDCheckAfterShutdown(unittest.TestCase): + + def test_process_terminates_as_expected( + self, + proc_info, + executable_under_test, + rmw_implementation_env, + rcl_assert_rmw_id_matches_env + ): + """Test that the executable under test terminates as expected.""" + assertion_method = self.assertEqual + if 'garbage' in (rmw_implementation_env, rcl_assert_rmw_id_matches_env): + assertion_method = self.assertNotEqual + executable_info = proc_info[executable_under_test] + assertion_method(executable_info.returncode, launch_testing.asserts.EXIT_OK) diff --git a/test/rcl/test_rmw_impl_id_check_exe.cpp b/test/rcl/test_rmw_impl_id_check_exe.cpp new file mode 100644 index 0000000..688b3e6 --- /dev/null +++ b/test/rcl/test_rmw_impl_id_check_exe.cpp @@ -0,0 +1,42 @@ +// 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 "rcl/rcl.h" + +int main(int, char **) +{ + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + rcl_ret_t ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + if (ret != RCL_RET_OK) { + return ret; + } + rcl_context_t context = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, &context); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_init_options_fini(&init_options); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_shutdown(&context); + if (ret != RCL_RET_OK) { + return ret; + } + ret = rcl_context_fini(&context); + if (ret != RCL_RET_OK) { + return ret; + } + return 0; +} diff --git a/test/rcl/test_rmw_impl_id_check_func.cpp b/test/rcl/test_rmw_impl_id_check_func.cpp new file mode 100644 index 0000000..62c3f2e --- /dev/null +++ b/test/rcl/test_rmw_impl_id_check_func.cpp @@ -0,0 +1,157 @@ +// 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 "rcutils/env.h" +#include "rcutils/strdup.h" + +#include "rcl/allocator.h" +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl/rmw_implementation_identifier_check.h" + +#include "../mocking_utils/patch.hpp" + +TEST(TestRmwCheck, test_rmw_check_id_impl) { + EXPECT_EQ(RCL_RET_OK, rcl_rmw_implementation_identifier_check()); +} + +TEST(TestRmwCheck, test_failing_configuration) { + const char * expected_rmw_impl_env = NULL; + const char * expected_rmw_id_matches = NULL; + + const char * get_env_var_name = rcutils_get_env( + RMW_IMPLEMENTATION_ENV_VAR_NAME, + &expected_rmw_impl_env); + EXPECT_FALSE(get_env_var_name); + + const char * get_env_id_matches_name = rcutils_get_env( + RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, + &expected_rmw_id_matches); + EXPECT_FALSE(get_env_id_matches_name); + + // Fail test case, reason: RMW_IMPLEMENTATION_ENV_VAR_NAME set, not matching rmw impl + EXPECT_TRUE(rcutils_set_env(RMW_IMPLEMENTATION_ENV_VAR_NAME, "some_random_name")); + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, "")); + EXPECT_EQ(RCL_RET_MISMATCHED_RMW_ID, rcl_rmw_implementation_identifier_check()); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Fail test case, reason: RMW_IMPLEMENTATION_ENV_VAR_NAME set, not matching rmw impl + EXPECT_TRUE(rcutils_set_env(RMW_IMPLEMENTATION_ENV_VAR_NAME, "")); + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, "some_random_name")); + EXPECT_EQ(RCL_RET_MISMATCHED_RMW_ID, rcl_rmw_implementation_identifier_check()); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Fail test case, reason: env variables not equal + EXPECT_TRUE(rcutils_set_env(RMW_IMPLEMENTATION_ENV_VAR_NAME, "some_random_name")); + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, "diff_random")); + EXPECT_EQ(RCL_RET_ERROR, rcl_rmw_implementation_identifier_check()); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Fail test case, reason: equal env variables do not match rmw impl + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, "some_random_name")); + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, "some_random_name")); + EXPECT_EQ(RCL_RET_MISMATCHED_RMW_ID, rcl_rmw_implementation_identifier_check()); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // Restore env variables set in the test + EXPECT_TRUE(rcutils_set_env(RMW_IMPLEMENTATION_ENV_VAR_NAME, expected_rmw_impl_env)); + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, expected_rmw_id_matches)); +} + +// Define dummy comparison operators for rcutils_allocator_t type for use with the Mimick Library +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) + +// Mock internal calls to external libraries to fail +TEST(TestRmwCheck, test_mock_rmw_impl_check) { + { + // Fail reading RMW_IMPLEMENTATION_ENV_VAR_NAME + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_get_env, "invalid arg"); + EXPECT_EQ(RCL_RET_ERROR, rcl_rmw_implementation_identifier_check()); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + // Fail copying RMW_IMPLEMENTATION_ENV_VAR_NAME env result + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_strdup, static_cast(NULL)); + EXPECT_EQ(RCL_RET_BAD_ALLOC, rcl_rmw_implementation_identifier_check()); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + // Fail reading RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME + auto mock = mocking_utils::patch( + "lib:rcl", rcutils_get_env, [](auto, const char ** env_value) { + static int counter = 1; + *env_value = ""; + if (counter == 1) { + counter++; + return static_cast(NULL); + } else { + return "argument env_value is null"; + } + }); + EXPECT_EQ(RCL_RET_ERROR, rcl_rmw_implementation_identifier_check()); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + // Fail copying RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME env result + // Set the variable, as is not set by default + const char * expected_rmw_id_matches = NULL; + const char * get_env_id_matches_name = rcutils_get_env( + RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, + &expected_rmw_id_matches); + EXPECT_EQ(NULL, get_env_id_matches_name); + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, "some_random_name")); + + auto mock = mocking_utils::patch( + "lib:rcl", rcutils_strdup, + [](const char * str, auto allocator) + { + static int counter = 1; + if (counter == 1) { + counter++; + char * dup = static_cast( + allocator.allocate(strlen(str) + 1, allocator.state)); + memcpy(dup, str, strlen(str) + 1); + return dup; + } else { + return static_cast(NULL); + } + }); + EXPECT_EQ(RCL_RET_BAD_ALLOC, rcl_rmw_implementation_identifier_check()); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + EXPECT_TRUE(rcutils_set_env(RCL_ASSERT_RMW_ID_MATCHES_ENV_VAR_NAME, expected_rmw_id_matches)); + } + { + // Fail reading rmw_impl_identifier + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_get_implementation_identifier, static_cast(NULL)); + EXPECT_EQ(RCL_RET_ERROR, rcl_rmw_implementation_identifier_check()); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } +} diff --git a/test/rcl/test_security.cpp b/test/rcl/test_security.cpp new file mode 100644 index 0000000..7eca4c5 --- /dev/null +++ b/test/rcl/test_security.cpp @@ -0,0 +1,406 @@ +// Copyright 2018-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 +#include +#include +#include + +#include "rcl/security.h" +#include "rcl/error_handling.h" + +#include "rcutils/env.h" +#include "rcutils/filesystem.h" + +#include "rmw/error_handling.h" +#include "rmw/rmw.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" + +#include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" + +#define TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME "/test_security_directory" +#define TEST_ENCLAVE "dummy_enclave" +#define TEST_ENCLAVE_ABSOLUTE "/" TEST_ENCLAVE + +#ifndef _WIN32 +# define PATH_SEPARATOR "/" +#else +# define PATH_SEPARATOR "\\" +#endif + +#define TEST_ENCLAVE_MULTIPLE_TOKENS_ABSOLUTE \ + "/group1" TEST_ENCLAVE_ABSOLUTE +#define TEST_ENCLAVE_MULTIPLE_TOKENS_DIR \ + "group1" PATH_SEPARATOR TEST_ENCLAVE + +char g_envstring[512] = {0}; + +static int putenv_wrapper(const char * env_var) +{ +#ifdef _WIN32 + return _putenv(env_var); +#else + return putenv(const_cast(env_var)); +#endif +} + +static int unsetenv_wrapper(const char * var_name) +{ +#ifdef _WIN32 + // On windows, putenv("VAR=") deletes VAR from environment + std::string var(var_name); + var += "="; + return _putenv(var.c_str()); +#else + return unsetenv(var_name); +#endif +} + +class TestGetSecureRoot : public ::testing::Test +{ +protected: + void SetUp() final + { + // Reset rcutil error global state in case a previously + // running test has failed. + rcl_reset_error(); + + // Always make sure the variable we set is unset at the beginning of a test + unsetenv_wrapper(ROS_SECURITY_KEYSTORE_VAR_NAME); + unsetenv_wrapper(ROS_SECURITY_ENCLAVE_OVERRIDE); + unsetenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME); + unsetenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME); + allocator = rcl_get_default_allocator(); + root_path = nullptr; + secure_root = nullptr; + base_lookup_dir_fqn = nullptr; + } + + void TearDown() final + { + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + allocator.deallocate(root_path, allocator.state); + }); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + allocator.deallocate(secure_root, allocator.state); + }); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + allocator.deallocate(base_lookup_dir_fqn, allocator.state); + }); + } + + void set_base_lookup_dir_fqn(const char * resource_dir, const char * resource_dir_name) + { + base_lookup_dir_fqn = rcutils_join_path( + resource_dir, resource_dir_name, allocator); + std::string putenv_input = ROS_SECURITY_KEYSTORE_VAR_NAME "="; + putenv_input += base_lookup_dir_fqn; + memcpy( + g_envstring, putenv_input.c_str(), + std::min(putenv_input.length(), sizeof(g_envstring) - 1)); + putenv_wrapper(g_envstring); + } + + rcl_allocator_t allocator; + char * root_path; + char * secure_root; + char * base_lookup_dir_fqn; +}; + +TEST_F(TestGetSecureRoot, failureScenarios) { + EXPECT_EQ( + rcl_get_secure_root(nullptr, &allocator), + (char *) NULL); + rcl_reset_error(); + + EXPECT_EQ( + rcl_get_secure_root(TEST_ENCLAVE_ABSOLUTE, &allocator), + (char *) NULL); + rcl_reset_error(); + + putenv_wrapper(ROS_SECURITY_KEYSTORE_VAR_NAME "=" TEST_RESOURCES_DIRECTORY); + + /* Security directory is set, but there's no matching directory */ + /// Wrong enclave + EXPECT_EQ( + rcl_get_secure_root("some_other_enclave", &allocator), + (char *) NULL); + rcl_reset_error(); +} + +TEST_F(TestGetSecureRoot, successScenarios_local_root_enclave) { + putenv_wrapper( + ROS_SECURITY_KEYSTORE_VAR_NAME "=" + TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME); + + secure_root = rcl_get_secure_root("/", &allocator); + ASSERT_NE(nullptr, secure_root); + ASSERT_STREQ( + TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME PATH_SEPARATOR "enclaves", + secure_root); +} + +TEST_F(TestGetSecureRoot, successScenarios_local_exactMatch) { + putenv_wrapper( + ROS_SECURITY_KEYSTORE_VAR_NAME "=" + TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME); + + secure_root = rcl_get_secure_root(TEST_ENCLAVE_ABSOLUTE, &allocator); + ASSERT_NE(nullptr, secure_root); + std::string secure_root_str(secure_root); + ASSERT_STREQ( + TEST_ENCLAVE, + secure_root_str.substr(secure_root_str.size() - strlen(TEST_ENCLAVE)).c_str()); +} + +TEST_F(TestGetSecureRoot, successScenarios_local_exactMatch_multipleTokensName) { + putenv_wrapper( + ROS_SECURITY_KEYSTORE_VAR_NAME "=" + TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME); + + secure_root = rcl_get_secure_root( + TEST_ENCLAVE_MULTIPLE_TOKENS_ABSOLUTE, &allocator); + ASSERT_NE(nullptr, secure_root); + std::string secure_root_str(secure_root); + ASSERT_STREQ( + TEST_ENCLAVE, + secure_root_str.substr(secure_root_str.size() - strlen(TEST_ENCLAVE)).c_str()); +} + +TEST_F(TestGetSecureRoot, nodeSecurityEnclaveOverride_validEnclave) { + putenv_wrapper( + ROS_SECURITY_KEYSTORE_VAR_NAME "=" + TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME); + + /* Specify a valid enclave */ + putenv_wrapper(ROS_SECURITY_ENCLAVE_OVERRIDE "=" TEST_ENCLAVE_ABSOLUTE); + root_path = rcl_get_secure_root( + "name shouldn't matter", &allocator); + ASSERT_STREQ( + TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME + PATH_SEPARATOR "enclaves" PATH_SEPARATOR TEST_ENCLAVE, + root_path); +} + +TEST_F(TestGetSecureRoot, nodeSecurityEnclaveOverride_invalidEnclave) { + putenv_wrapper( + ROS_SECURITY_KEYSTORE_VAR_NAME "=" + TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME); + + /* The override provided should exist. Providing correct node/namespace/root dir won't help + * if the node override is invalid. */ + putenv_wrapper( + ROS_SECURITY_ENCLAVE_OVERRIDE + "=TheresN_oWayThi_sEnclave_Exists_hence_this_should_fail"); + EXPECT_EQ( + rcl_get_secure_root(TEST_ENCLAVE_ABSOLUTE, &allocator), + (char *) NULL); +} + +TEST_F(TestGetSecureRoot, test_get_security_options) { + /* The override provided should exist. Providing correct enclave name/root dir + * won't help if the node override is invalid. */ + rmw_security_options_t options = rmw_get_zero_initialized_security_options(); + putenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME "=false"); + rcl_ret_t ret = rcl_get_security_options_from_environment( + "doesn't matter at all", &allocator, &options); + ASSERT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; + EXPECT_EQ(RMW_SECURITY_ENFORCEMENT_PERMISSIVE, options.enforce_security); + EXPECT_EQ(NULL, options.security_root_path); + + putenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME "=true"); + putenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME "=Enforce"); + putenv_wrapper( + ROS_SECURITY_KEYSTORE_VAR_NAME "=" + TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME); + + putenv_wrapper( + ROS_SECURITY_ENCLAVE_OVERRIDE "=" TEST_ENCLAVE_MULTIPLE_TOKENS_ABSOLUTE); + ret = rcl_get_security_options_from_environment( + "doesn't matter at all", &allocator, &options); + ASSERT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; + EXPECT_EQ(RMW_SECURITY_ENFORCEMENT_ENFORCE, options.enforce_security); + EXPECT_STREQ( + TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME + PATH_SEPARATOR "enclaves" PATH_SEPARATOR TEST_ENCLAVE_MULTIPLE_TOKENS_DIR, + options.security_root_path); + EXPECT_EQ(RMW_RET_OK, rmw_security_options_fini(&options, &allocator)); + + options = rmw_get_zero_initialized_security_options(); + unsetenv_wrapper(ROS_SECURITY_ENCLAVE_OVERRIDE); + putenv_wrapper( + ROS_SECURITY_KEYSTORE_VAR_NAME "=" + TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME); + ret = rcl_get_security_options_from_environment( + TEST_ENCLAVE_ABSOLUTE, &allocator, &options); + ASSERT_EQ(RMW_RET_OK, ret) << rmw_get_error_string().str; + EXPECT_EQ(RMW_SECURITY_ENFORCEMENT_ENFORCE, options.enforce_security); + EXPECT_STREQ( + TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME + PATH_SEPARATOR "enclaves" PATH_SEPARATOR TEST_ENCLAVE, + options.security_root_path); + EXPECT_EQ(RMW_RET_OK, rmw_security_options_fini(&options, &allocator)); +} + +TEST_F(TestGetSecureRoot, test_rcl_security_enabled) { + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_security_enabled(nullptr)); + rcl_reset_error(); + + { + bool use_security; + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_get_env, "internal error"); + EXPECT_EQ(RCL_RET_ERROR, rcl_security_enabled(&use_security)); + rcl_reset_error(); + } + + { + bool use_security = false; + putenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME "=true"); + EXPECT_EQ(RCL_RET_OK, rcl_security_enabled(&use_security)); + EXPECT_TRUE(use_security); + unsetenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME); + } + + { + bool use_security = true; + putenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME "=false"); + EXPECT_EQ(RCL_RET_OK, rcl_security_enabled(&use_security)); + EXPECT_FALSE(use_security); + unsetenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME); + } + + { + bool use_security = true; + putenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME "=foo"); + EXPECT_EQ(RCL_RET_OK, rcl_security_enabled(&use_security)); + EXPECT_FALSE(use_security); + unsetenv_wrapper(ROS_SECURITY_ENABLE_VAR_NAME); + } + + { + bool use_security = true; + EXPECT_EQ(RCL_RET_OK, rcl_security_enabled(&use_security)); + EXPECT_FALSE(use_security); + } +} + +TEST_F(TestGetSecureRoot, test_rcl_get_enforcement_policy) { + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_get_enforcement_policy(nullptr)); + rcl_reset_error(); + + { + rmw_security_enforcement_policy_t policy; + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_get_env, "internal error"); + EXPECT_EQ(RCL_RET_ERROR, rcl_get_enforcement_policy(&policy)); + rcl_reset_error(); + } + + { + rmw_security_enforcement_policy_t policy = RMW_SECURITY_ENFORCEMENT_PERMISSIVE; + putenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME "=Enforce"); + EXPECT_EQ(RCL_RET_OK, rcl_get_enforcement_policy(&policy)); + EXPECT_EQ(RMW_SECURITY_ENFORCEMENT_ENFORCE, policy); + unsetenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME); + } + + { + rmw_security_enforcement_policy_t policy = RMW_SECURITY_ENFORCEMENT_ENFORCE; + EXPECT_EQ(RCL_RET_OK, rcl_get_enforcement_policy(&policy)); + EXPECT_EQ(RMW_SECURITY_ENFORCEMENT_PERMISSIVE, policy); + } + + { + rmw_security_enforcement_policy_t policy = RMW_SECURITY_ENFORCEMENT_ENFORCE; + putenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME "=foo"); + EXPECT_EQ(RCL_RET_OK, rcl_get_enforcement_policy(&policy)); + EXPECT_EQ(RMW_SECURITY_ENFORCEMENT_PERMISSIVE, policy); + unsetenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME); + } + + { + rmw_security_enforcement_policy_t policy = RMW_SECURITY_ENFORCEMENT_ENFORCE; + putenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME "=ENFORCE"); + EXPECT_EQ(RCL_RET_OK, rcl_get_enforcement_policy(&policy)); + EXPECT_EQ(RMW_SECURITY_ENFORCEMENT_PERMISSIVE, policy); + unsetenv_wrapper(ROS_SECURITY_STRATEGY_VAR_NAME); + } +} + +TEST_F(TestGetSecureRoot, test_rcl_get_secure_root_with_bad_arguments) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + EXPECT_EQ(nullptr, rcl_get_secure_root(nullptr, &allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + EXPECT_EQ(nullptr, rcl_get_secure_root("test", nullptr)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + rcl_allocator_t invalid_allocator = rcutils_get_zero_initialized_allocator(); + EXPECT_EQ(nullptr, rcl_get_secure_root("test", &invalid_allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +TEST_F(TestGetSecureRoot, test_rcl_get_secure_root_with_internal_errors) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_allocator_t failing_allocator = get_time_bombed_allocator(); + + std::map env; + auto mock = mocking_utils::patch( + "lib:rcl", rcutils_get_env, + [&](const char * name, const char ** value) -> const char * { + if (env.count(name) == 0) { + return "internal error"; + } + *value = env[name].c_str(); + return nullptr; + }); + + // fail to get ROS_SECURITY_KEYSTORE_VAR_NAME from environment + EXPECT_EQ(nullptr, rcl_get_secure_root("test", &allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + env[ROS_SECURITY_KEYSTORE_VAR_NAME] = + TEST_RESOURCES_DIRECTORY TEST_SECURITY_DIRECTORY_RESOURCES_DIR_NAME; + + // fail to copy ROS_SECURITY_KEYSTORE_VAR_NAME value + set_time_bombed_allocator_count(failing_allocator, 0); + EXPECT_EQ(nullptr, rcl_get_secure_root("test", &failing_allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + // fail to get ROS_SECURITY_ENCLAVE_OVERRIDE from environment + EXPECT_EQ(nullptr, rcl_get_secure_root("test", &allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + env[ROS_SECURITY_ENCLAVE_OVERRIDE] = TEST_ENCLAVE_ABSOLUTE; + + // fail to copy ROS_SECURITY_ENCLAVE_OVERRIDE value + set_time_bombed_allocator_count(failing_allocator, 1); + EXPECT_EQ(nullptr, rcl_get_secure_root("test", &failing_allocator)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} diff --git a/test/rcl/test_service.cpp b/test/rcl/test_service.cpp new file mode 100644 index 0000000..047eb3e --- /dev/null +++ b/test/rcl/test_service.cpp @@ -0,0 +1,651 @@ +// Copyright 2015 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 "rcl/service.h" +#include "rcl/rcl.h" + +#include "test_msgs/srv/basic_types.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rmw/validate_namespace.h" + +#include "wait_for_entity_helpers.hpp" +#include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestServiceFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "test_service_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +/* Basic nominal test of a service. + */ +TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal) { + rcl_ret_t ret; + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + const char * topic = "primitives"; + const char * expected_topic = "/primitives"; + + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << rcl_get_error_string().str; + + const rmw_qos_profile_t * request_subscription_qos = + rcl_service_request_subscription_get_actual_qos(&service); + EXPECT_EQ(rmw_qos_profile_services_default.reliability, request_subscription_qos->reliability); + EXPECT_EQ(rmw_qos_profile_services_default.history, request_subscription_qos->history); + EXPECT_EQ(rmw_qos_profile_services_default.depth, request_subscription_qos->depth); + EXPECT_EQ(rmw_qos_profile_services_default.durability, request_subscription_qos->durability); + + const rmw_qos_profile_t * response_publisher_qos = + rcl_service_response_publisher_get_actual_qos(&service); + EXPECT_EQ(rmw_qos_profile_services_default.reliability, response_publisher_qos->reliability); + EXPECT_EQ(rmw_qos_profile_services_default.history, response_publisher_qos->history); + EXPECT_EQ(rmw_qos_profile_services_default.depth, response_publisher_qos->depth); + EXPECT_EQ(rmw_qos_profile_services_default.durability, response_publisher_qos->durability); + + ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Check if null service is valid + EXPECT_FALSE(rcl_service_is_valid(nullptr)); + rcl_reset_error(); + + // Check if zero initialized client is valid + service = rcl_get_zero_initialized_service(); + EXPECT_FALSE(rcl_service_is_valid(&service)); + rcl_reset_error(); + + // Check that a valid service is valid + service = rcl_get_zero_initialized_service(); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_service_is_valid(&service)); + rcl_reset_error(); + + // Check that the service name matches what we assigned. + EXPECT_EQ(strcmp(rcl_service_get_service_name(&service), expected_topic), 0); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options = rcl_client_get_default_options(); + ret = rcl_client_init(&client, this->node_ptr, ts, topic, &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + ASSERT_TRUE(wait_for_server_to_be_available(this->node_ptr, &client, 10, 1000)); + + // Initialize a request. + test_msgs__srv__BasicTypes_Request client_request; + test_msgs__srv__BasicTypes_Request__init(&client_request); + // TODO(clalancette): the C __init methods do not initialize all structure + // members, so the numbers in the fields not explicitly set is arbitrary. + // The CDR deserialization in Fast-CDR requires a 0 or 1 for bool fields, + // so we explicitly initialize that even though we don't use it. This can be + // removed once the C __init methods initialize all members by default. + client_request.bool_value = false; + client_request.uint8_value = 1; + client_request.uint32_value = 2; + int64_t sequence_number; + rcutils_time_point_value_t start_timestamp; + // take timestamp before sending request + EXPECT_EQ(RCUTILS_RET_OK, rcutils_system_time_now(&start_timestamp)); + ret = rcl_send_request(&client, &client_request, &sequence_number); + EXPECT_EQ(sequence_number, 1); + test_msgs__srv__BasicTypes_Request__fini(&client_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ASSERT_TRUE(wait_for_service_to_be_ready(&service, context_ptr, 10, 100)); + + // This scope simulates the service responding in a different context so that we can + // test take_request/send_response in a single-threaded, deterministic execution. + { + // Initialize a response. + test_msgs__srv__BasicTypes_Response service_response; + test_msgs__srv__BasicTypes_Response__init(&service_response); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__srv__BasicTypes_Response__fini(&service_response); + }); + + // Initialize a separate instance of the request and take the pending request. + test_msgs__srv__BasicTypes_Request service_request; + test_msgs__srv__BasicTypes_Request__init(&service_request); + rmw_service_info_t header; + ret = rcl_take_request_with_info(&service, &header, &service_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_EQ(1, service_request.uint8_value); + EXPECT_EQ(2UL, service_request.uint32_value); +#ifdef RMW_TIMESTAMPS_SUPPORTED + EXPECT_GE(header.source_timestamp, start_timestamp); +#ifdef RMW_RECEIVED_TIMESTAMP_SUPPORTED + EXPECT_GE(header.received_timestamp, start_timestamp); + EXPECT_GE(header.received_timestamp, header.source_timestamp); +#else + EXPECT_EQ(0u, header.received_timestamp); +#endif +#else + EXPECT_EQ(0u, header.source_timestamp); + EXPECT_EQ(0u, header.received_timestamp); +#endif + // Simulate a response callback by summing the request and send the response.. + service_response.uint64_value = service_request.uint8_value + service_request.uint32_value; + // take new timestamp before sending response + EXPECT_EQ(RCUTILS_RET_OK, rcutils_system_time_now(&start_timestamp)); + ret = rcl_send_response(&service, &header.request_id, &service_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + test_msgs__srv__BasicTypes_Request__fini(&service_request); + } + ASSERT_FALSE(wait_for_service_to_be_ready(&service, context_ptr, 10, 100)); + + // Initialize the response owned by the client and take the response. + test_msgs__srv__BasicTypes_Response client_response; + test_msgs__srv__BasicTypes_Response__init(&client_response); + + rmw_service_info_t header; + ret = rcl_take_response_with_info(&client, &header, &client_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(client_response.uint64_value, 3ULL); + EXPECT_EQ(header.request_id.sequence_number, 1); +#ifdef RMW_TIMESTAMPS_SUPPORTED + EXPECT_GE(header.source_timestamp, start_timestamp); +#ifdef RMW_RECEIVED_TIMESTAMP_SUPPORTED + EXPECT_GE(header.received_timestamp, start_timestamp); + EXPECT_GE(header.received_timestamp, header.source_timestamp); +#else + EXPECT_EQ(0u, header.received_timestamp); +#endif +#else + EXPECT_EQ(0u, header.source_timestamp); + EXPECT_EQ(0u, header.received_timestamp); +#endif + + ret = rcl_take_response_with_info(&client, &header, &client_response); + EXPECT_EQ(RCL_RET_CLIENT_TAKE_FAILED, ret) << rcl_get_error_string().str; + + test_msgs__srv__BasicTypes_Response__fini(&client_response); +} + +/* Basic nominal test of a service with rcl_take_response + */ +TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_without_info) { + rcl_ret_t ret; + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + const char * topic = "primitives"; + const char * expected_topic = "/primitives"; + + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_service_is_valid(&service)); + rcl_reset_error(); + + // Check that the service name matches what we assigned. + EXPECT_STREQ(rcl_service_get_service_name(&service), expected_topic); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_client_t client = rcl_get_zero_initialized_client(); + rcl_client_options_t client_options = rcl_client_get_default_options(); + ret = rcl_client_init(&client, this->node_ptr, ts, topic, &client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_client_fini(&client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + ASSERT_TRUE(wait_for_server_to_be_available(this->node_ptr, &client, 10, 1000)); + + // Initialize a request. + test_msgs__srv__BasicTypes_Request client_request; + test_msgs__srv__BasicTypes_Request__init(&client_request); + // TODO(clalancette): the C __init methods do not initialize all structure + // members, so the numbers in the fields not explicitly set is arbitrary. + // The CDR deserialization in Fast-CDR requires a 0 or 1 for bool fields, + // so we explicitly initialize that even though we don't use it. This can be + // removed once the C __init methods initialize all members by default. + client_request.bool_value = false; + client_request.uint8_value = 1; + client_request.uint32_value = 2; + int64_t sequence_number = 0; + ret = rcl_send_request(&client, &client_request, &sequence_number); + EXPECT_NE(sequence_number, 0); + test_msgs__srv__BasicTypes_Request__fini(&client_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ASSERT_TRUE(wait_for_service_to_be_ready(&service, context_ptr, 10, 100)); + + // This scope simulates the service responding in a different context so that we can + // test take_request/send_response in a single-threaded, deterministic execution. + { + // Initialize a response. + test_msgs__srv__BasicTypes_Response service_response; + test_msgs__srv__BasicTypes_Response__init(&service_response); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__srv__BasicTypes_Response__fini(&service_response); + }); + + // Initialize a separate instance of the request and take the pending request. + test_msgs__srv__BasicTypes_Request service_request; + test_msgs__srv__BasicTypes_Request__init(&service_request); + rmw_service_info_t header; + ret = rcl_take_request(&service, &(header.request_id), &service_request); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_EQ(1, service_request.uint8_value); + EXPECT_EQ(2UL, service_request.uint32_value); + // Simulate a response callback by summing the request and send the response.. + service_response.uint64_value = service_request.uint8_value + service_request.uint32_value; + ret = rcl_send_response(&service, &header.request_id, &service_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + test_msgs__srv__BasicTypes_Request__fini(&service_request); + } + ASSERT_FALSE(wait_for_service_to_be_ready(&service, context_ptr, 10, 100)); + + // Initialize the response owned by the client and take the response. + test_msgs__srv__BasicTypes_Response client_response; + test_msgs__srv__BasicTypes_Response__init(&client_response); + + rmw_service_info_t header; + ret = rcl_take_response(&client, &(header.request_id), &client_response); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(client_response.uint64_value, 3ULL); + EXPECT_NE(header.request_id.sequence_number, 0); + + ret = rcl_take_response(&client, &(header.request_id), &client_response); + EXPECT_EQ(RCL_RET_CLIENT_TAKE_FAILED, ret) << rcl_get_error_string().str; + + test_msgs__srv__BasicTypes_Response__fini(&client_response); +} + +/* Passing bad/invalid arguments to service functions + */ +TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_bad_arguments) { + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + const char * topic = "primitives"; + + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + + rcl_service_options_t service_options_bad_alloc = rcl_service_get_default_options(); + service_options_bad_alloc.allocator.allocate = nullptr; + rcl_node_t invalid_node = rcl_get_zero_initialized_node(); + + EXPECT_EQ( + RCL_RET_NODE_INVALID, rcl_service_init( + &service, nullptr, ts, topic, &service_options)) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_NODE_INVALID, rcl_service_init( + &service, &invalid_node, ts, topic, &service_options)) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_service_init( + nullptr, this->node_ptr, ts, topic, &service_options)) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_service_init( + &service, this->node_ptr, nullptr, topic, &service_options)) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_service_init( + &service, this->node_ptr, ts, nullptr, &service_options)) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_service_init( + &service, this->node_ptr, ts, topic, nullptr)) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_service_init( + &service, this->node_ptr, ts, topic, + &service_options_bad_alloc)) << rcl_get_error_string().str; + + EXPECT_EQ( + RCL_RET_NODE_INVALID, rcl_service_fini(&service, nullptr)) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_NODE_INVALID, rcl_service_fini(&service, &invalid_node)) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_SERVICE_INVALID, rcl_service_fini( + nullptr, this->node_ptr)) << rcl_get_error_string().str; + + test_msgs__srv__BasicTypes_Request service_request; + test_msgs__srv__BasicTypes_Response service_response; + test_msgs__srv__BasicTypes_Request__init(&service_request); + test_msgs__srv__BasicTypes_Response__init(&service_response); + rmw_service_info_t header; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__srv__BasicTypes_Request__fini(&service_request); + test_msgs__srv__BasicTypes_Response__fini(&service_response); + }); + + EXPECT_EQ(nullptr, rcl_service_get_service_name(nullptr)); + EXPECT_EQ(nullptr, rcl_service_get_options(nullptr)); + EXPECT_EQ(nullptr, rcl_service_get_rmw_handle(nullptr)); + EXPECT_EQ( + RCL_RET_SERVICE_INVALID, rcl_take_request_with_info(nullptr, &header, &service_request)); + EXPECT_EQ( + RCL_RET_SERVICE_INVALID, rcl_send_response(nullptr, &header.request_id, &service_response)); + EXPECT_EQ( + RCL_RET_SERVICE_INVALID, rcl_take_request(nullptr, &(header.request_id), &service_request)); + EXPECT_EQ(nullptr, rcl_service_request_subscription_get_actual_qos(nullptr)); + EXPECT_EQ(nullptr, rcl_service_response_publisher_get_actual_qos(nullptr)); + + EXPECT_EQ(nullptr, rcl_service_get_service_name(&service)); + EXPECT_EQ(nullptr, rcl_service_get_options(&service)); + EXPECT_EQ(nullptr, rcl_service_get_rmw_handle(&service)); + EXPECT_EQ( + RCL_RET_SERVICE_INVALID, rcl_take_request_with_info(&service, &header, &service_request)); + EXPECT_EQ( + RCL_RET_SERVICE_INVALID, rcl_send_response(&service, &(header.request_id), &service_response)); + EXPECT_EQ( + RCL_RET_SERVICE_INVALID, rcl_take_request(&service, &(header.request_id), &service_request)); + + service_options_bad_alloc.allocator = get_failing_allocator(); + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_service_init( + &service, this->node_ptr, ts, + topic, &service_options_bad_alloc)) << rcl_get_error_string().str; + + EXPECT_EQ(nullptr, rcl_service_request_subscription_get_actual_qos(&service)); + EXPECT_EQ(nullptr, rcl_service_response_publisher_get_actual_qos(&service)); +} + +/* Name failed tests + */ +TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_fail_name) { + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + const char * topic = "white space"; + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + rcl_ret_t ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_SERVICE_NAME_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + const char * topic2 = "{invalidbecausecurlybraces}"; + ret = rcl_service_init(&service, this->node_ptr, ts, topic2, &service_options); + EXPECT_EQ(RCL_RET_SERVICE_NAME_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); +} + +// Define dummy comparison operators for rcutils_allocator_t type for use with the Mimick Library +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) + +/* Test failed service initialization using mocks + */ +TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_fail_ini_mocked) { + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + constexpr char topic[] = "topic"; + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + service_options.qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + rcl_ret_t ret = RCL_RET_OK; + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_string_map_init, RCUTILS_RET_ERROR); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + // Mocking this function causes rcl_expand_topic_name to return RCL_RET_ERROR + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_validate_namespace, RMW_RET_ERROR); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + auto mock = mocking_utils::inject_on_return( + "lib:rcl", rcutils_string_map_fini, RCUTILS_RET_ERROR); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_validate_full_topic_name, RMW_RET_ERROR); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + auto mock = mocking_utils::patch( + "lib:rcl", rmw_validate_full_topic_name, + [](auto, int * result, auto) { + *result = RMW_TOPIC_INVALID_IS_EMPTY_STRING; + return RMW_RET_OK; + }); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_SERVICE_NAME_INVALID, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_create_service, nullptr); + ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } +} + +/* Test failed service finalization using mocks + */ +TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_fail_fini_mocked) { + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + constexpr char topic[] = "primitives"; + + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + rcl_ret_t ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_service_t empty_service = rcl_get_zero_initialized_service(); + ret = rcl_service_fini(&empty_service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + auto mock = mocking_utils::inject_on_return( + "lib:rcl", rmw_destroy_service, RMW_RET_ERROR); + ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_ERROR, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +/* Test failed service take_request_with_info using mocks and nullptrs + */ +TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_fail_take_request_with_info) { + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + constexpr char topic[] = "primitives"; + + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + rcl_ret_t ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + test_msgs__srv__BasicTypes_Request service_request; + test_msgs__srv__BasicTypes_Request__init(&service_request); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__srv__BasicTypes_Request__fini(&service_request); + }); + rmw_service_info_t header; + + ret = rcl_take_request_with_info(nullptr, &header, &service_request); + EXPECT_EQ(RCL_RET_SERVICE_INVALID, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + ret = rcl_take_request_with_info(&service, nullptr, &service_request); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + ret = rcl_take_request_with_info(&service, &header, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_take_request, RMW_RET_ERROR); + ret = rcl_take_request_with_info(&service, &header, &service_request); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_take_request, RMW_RET_BAD_ALLOC); + ret = rcl_take_request_with_info(&service, &header, &service_request); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + { + auto mock = mocking_utils::patch( + "lib:rcl", rmw_take_request, + [](auto, auto, auto, bool * taken) { + *taken = false; + return RMW_RET_OK; + }); + ret = rcl_take_request_with_info(&service, &header, &service_request); + EXPECT_EQ(RCL_RET_SERVICE_TAKE_FAILED, ret); + } +} + +/* Test failed service send_response using mocks and nullptrs + */ +TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_fail_send_response) { + const rosidl_service_type_support_t * ts = ROSIDL_GET_SRV_TYPE_SUPPORT( + test_msgs, srv, BasicTypes); + constexpr char topic[] = "primitives"; + + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + rcl_ret_t ret = rcl_service_init(&service, this->node_ptr, ts, topic, &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + // Init dummy response. + test_msgs__srv__BasicTypes_Response service_response; + test_msgs__srv__BasicTypes_Response__init(&service_response); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__srv__BasicTypes_Response__fini(&service_response); + }); + rmw_service_info_t header; + + ret = rcl_send_response(nullptr, &header.request_id, &service_response); + EXPECT_EQ(RCL_RET_SERVICE_INVALID, ret); + + ret = rcl_send_response(&service, nullptr, &service_response); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + + ret = rcl_send_response(&service, &header.request_id, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_send_response, RMW_RET_ERROR); + ret = rcl_send_response(&service, &header.request_id, &service_response); + EXPECT_EQ(RCL_RET_ERROR, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } +} diff --git a/test/rcl/test_subscription.cpp b/test/rcl/test_subscription.cpp new file mode 100644 index 0000000..46bd152 --- /dev/null +++ b/test/rcl/test_subscription.cpp @@ -0,0 +1,1705 @@ +// Copyright 2015 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 "rcl/subscription.h" +#include "rcl/rcl.h" +#include "rmw/rmw.h" +#include "rmw/validate_full_topic_name.h" + +#include "rcutils/strdup.h" +#include "rcutils/testing/fault_injection.h" +#include "test_msgs/msg/basic_types.h" +#include "test_msgs/msg/strings.h" +#include "rosidl_runtime_c/string_functions.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rcl/node.h" +#include "rcutils/env.h" +#include "wait_for_entity_helpers.hpp" + +#include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +class CLASSNAME (TestSubscriptionFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + constexpr char name[] = "test_subscription_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +class CLASSNAME (TestSubscriptionFixtureInit, RMW_IMPLEMENTATION) + : public CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION) +{ +public: + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + const char * topic = "/chatter"; + rcl_subscription_options_t subscription_options; + rcl_subscription_t subscription; + rcl_subscription_t subscription_zero_init; + rcl_ret_t ret; + rcutils_allocator_t allocator; + + void SetUp() override + { + CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION) ::SetUp(); + allocator = rcutils_get_default_allocator(); + subscription_options = rcl_subscription_get_default_options(); + subscription = rcl_get_zero_initialized_subscription(); + subscription_zero_init = rcl_get_zero_initialized_subscription(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() override + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION) ::TearDown(); + } +}; + +/* Test subscription init, fini and is_valid functions + */ +TEST_F( + CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), + test_subscription_init_fini_and_is_valid) +{ + rcl_ret_t ret; + + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic[] = "chatter"; + constexpr char expected_topic[] = "/chatter"; + + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(strcmp(rcl_subscription_get_topic_name(&subscription), expected_topic), 0); + ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Test is_valid for subscription with nullptr + EXPECT_FALSE(rcl_subscription_is_valid(nullptr)); + rcl_reset_error(); + + // Test is_valid for zero initialized subscription + subscription = rcl_get_zero_initialized_subscription(); + EXPECT_FALSE(rcl_subscription_is_valid(&subscription)); + rcl_reset_error(); +} + +// Define dummy comparison operators for rcutils_allocator_t type +// to use with the Mimick mocking library +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, ==) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, !=) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, <) +MOCKING_UTILS_BOOL_OPERATOR_RETURNS_FALSE(rcutils_allocator_t, >) + +// Bad arguments for init and fini +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_bad_init) { + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic[] = "/chatter"; + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_node_t invalid_node = rcl_get_zero_initialized_node(); + + ASSERT_FALSE(rcl_node_is_valid_except_context(&invalid_node)); + rcl_reset_error(); + + EXPECT_EQ(nullptr, rcl_node_get_rmw_handle(&invalid_node)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_NODE_INVALID, + rcl_subscription_init(&subscription, nullptr, ts, topic, &subscription_options)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_NODE_INVALID, + rcl_subscription_init(&subscription, &invalid_node, ts, topic, &subscription_options)); + rcl_reset_error(); + + rcl_ret_t ret = rcl_subscription_init( + &subscription, this->node_ptr, ts, "spaced name", &subscription_options); + EXPECT_EQ(RCL_RET_TOPIC_NAME_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_subscription_init( + &subscription, this->node_ptr, ts, "sub{ros_not_match}", &subscription_options); + EXPECT_EQ(RCL_RET_TOPIC_NAME_INVALID, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + { + rcutils_ret_t rcutils_string_map_init_returns = RCUTILS_RET_BAD_ALLOC; + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rcutils_string_map_init, rcutils_string_map_init_returns); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + rcl_reset_error(); + + rcutils_string_map_init_returns = RCUTILS_RET_ERROR; + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + { + auto mock = + mocking_utils::inject_on_return("lib:rcl", rcutils_string_map_fini, RCUTILS_RET_ERROR); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + { + rmw_ret_t rmw_validate_full_topic_name_returns = RMW_RET_OK; + auto mock = mocking_utils::patch( + "lib:rcl", rmw_validate_full_topic_name, + [&](auto, int * result, auto) { + *result = RMW_TOPIC_INVALID_TOO_LONG; + return rmw_validate_full_topic_name_returns; + }); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_TOPIC_NAME_INVALID, ret); + rcl_reset_error(); + + rmw_validate_full_topic_name_returns = RMW_RET_ERROR; + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + { + auto mock = + mocking_utils::patch_and_return("lib:rcl", rmw_create_subscription, nullptr); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + { + auto mock = + mocking_utils::patch_and_return("lib:rcl", rmw_subscription_get_actual_qos, RMW_RET_ERROR); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + } + + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_TRUE(rcl_subscription_is_valid(&subscription)); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_SUBSCRIPTION_INVALID, rcl_subscription_fini(nullptr, this->node_ptr)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_NODE_INVALID, rcl_subscription_fini(&subscription, nullptr)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_NODE_INVALID, rcl_subscription_fini(&subscription, &invalid_node)); + rcl_reset_error(); + + auto mock = + mocking_utils::inject_on_return("lib:rcl", rmw_destroy_subscription, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_ERROR, rcl_subscription_fini(&subscription, this->node_ptr)); + rcl_reset_error(); + + // Make sure finalization completed anyways. + ASSERT_EQ(NULL, subscription.impl); +} + +/* Basic nominal test of a subscription + */ +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_nominal) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic[] = "/chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_reset_error(); + + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); +#ifdef RMW_TIMESTAMPS_SUPPORTED + rcl_time_point_value_t pre_publish_time; + EXPECT_EQ( + RCUTILS_RET_OK, + rcutils_system_time_now(&pre_publish_time)) << " could not get system time failed"; +#endif + { + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + msg.int64_value = 42; + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__BasicTypes__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + { + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__BasicTypes__fini(&msg); + }); + rmw_message_info_t message_info = rmw_get_zero_initialized_message_info(); + ret = rcl_take(&subscription, &msg, &message_info, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(42, msg.int64_value); + #ifdef RMW_TIMESTAMPS_SUPPORTED + EXPECT_NE(0u, message_info.source_timestamp); + EXPECT_TRUE(pre_publish_time <= message_info.source_timestamp) << + pre_publish_time << " > " << message_info.source_timestamp; + #ifdef RMW_RECEIVED_TIMESTAMP_SUPPORTED + EXPECT_NE(0u, message_info.received_timestamp); + EXPECT_TRUE(pre_publish_time <= message_info.received_timestamp); + EXPECT_TRUE(message_info.source_timestamp <= message_info.received_timestamp); + #else + EXPECT_EQ(0u, message_info.received_timestamp); + #endif + #else + EXPECT_EQ(0u, message_info.source_timestamp); + EXPECT_EQ(0u, message_info.received_timestamp); + #endif + } +} + +/* Basic nominal test of a publisher with a string. + */ +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_nominal_string) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic[] = "rcl_test_subscription_nominal_string_chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); + constexpr char test_string[] = "testing"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ(std::string(test_string), std::string(msg.string_value.data, msg.string_value.size)); + } +} + +/* Basic nominal test of a subscription taking a sequence. + */ +TEST_F( + CLASSNAME( + TestSubscriptionFixture, + RMW_IMPLEMENTATION), test_subscription_nominal_string_sequence) { + using namespace std::chrono_literals; + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic[] = "rcl_test_subscription_nominal_string_sequence_chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); + constexpr char test_string[] = "testing"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + auto allocator = rcutils_get_default_allocator(); + { + size_t size = 1; + rmw_message_info_sequence_t message_infos; + rmw_message_info_sequence_init(&message_infos, size, &allocator); + + rmw_message_sequence_t messages; + rmw_message_sequence_init(&messages, size, &allocator); + + auto seq = test_msgs__msg__Strings__Sequence__create(size); + + for (size_t ii = 0; ii < size; ++ii) { + messages.data[ii] = &seq->data[ii]; + } + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rmw_message_info_sequence_fini(&message_infos); + rmw_message_sequence_fini(&messages); + test_msgs__msg__Strings__Sequence__destroy(seq); + }); + + // Attempt to take more than capacity allows. + ret = rcl_take_sequence(&subscription, 5, &messages, &message_infos, nullptr); + ASSERT_EQ(RCL_RET_INVALID_ARGUMENT, ret) << rcl_get_error_string().str; + + ASSERT_EQ(0u, messages.size); + ASSERT_EQ(0u, message_infos.size); + } + + { + size_t size = 5; + rmw_message_info_sequence_t message_infos; + rmw_message_info_sequence_init(&message_infos, size, &allocator); + + rmw_message_sequence_t messages; + rmw_message_sequence_init(&messages, size, &allocator); + + auto seq = test_msgs__msg__Strings__Sequence__create(size); + + for (size_t ii = 0; ii < size; ++ii) { + messages.data[ii] = &seq->data[ii]; + } + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rmw_message_info_sequence_fini(&message_infos); + rmw_message_sequence_fini(&messages); + test_msgs__msg__Strings__Sequence__destroy(seq); + }); + + auto start = std::chrono::steady_clock::now(); + size_t total_messages_taken = 0u; + do { + // `wait_for_subscription_to_be_ready` only ensures there's one message ready, + // so we need to loop to guarantee that we get the three published messages. + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 1, 100)); + ret = rcl_take_sequence(&subscription, 5, &messages, &message_infos, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + total_messages_taken += messages.size; + EXPECT_EQ(messages.size, message_infos.size); + } while (total_messages_taken < 3 && std::chrono::steady_clock::now() < start + 10s); + + EXPECT_EQ(3u, total_messages_taken); + } + + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_publish(&publisher, &msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + // Give a brief moment for publications to go through. + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + // Take fewer messages than are available in the subscription + { + size_t size = 3; + rmw_message_info_sequence_t message_infos; + rmw_message_info_sequence_init(&message_infos, size, &allocator); + + rmw_message_sequence_t messages; + rmw_message_sequence_init(&messages, size, &allocator); + + auto seq = test_msgs__msg__Strings__Sequence__create(size); + + for (size_t ii = 0; ii < size; ++ii) { + messages.data[ii] = &seq->data[ii]; + } + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rmw_message_info_sequence_fini(&message_infos); + rmw_message_sequence_fini(&messages); + test_msgs__msg__Strings__Sequence__destroy(seq); + }); + + auto start = std::chrono::steady_clock::now(); + size_t total_messages_taken = 0u; + do { + // `wait_for_subscription_to_be_ready` only ensures there's one message ready, + // so we need to loop to guarantee that we get the three published messages. + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 1, 100)); + ret = rcl_take_sequence(&subscription, 3, &messages, &message_infos, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + total_messages_taken += messages.size; + EXPECT_EQ(messages.size, message_infos.size); + } while (total_messages_taken < 3 && std::chrono::steady_clock::now() < start + 10s); + + EXPECT_EQ(3u, total_messages_taken); + EXPECT_EQ( + std::string(test_string), + std::string(seq->data[0].string_value.data, seq->data[0].string_value.size)); + } +} + +/* Basic nominal test of a subscription with take_serialize msg + */ +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_serialized) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + rcutils_allocator_t allocator = rcl_get_default_allocator(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic[] = "/chatterSer"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); + auto initial_capacity_ser = 0u; + ASSERT_EQ( + RCL_RET_OK, rmw_serialized_message_init( + &serialized_msg, initial_capacity_ser, &allocator)) << rcl_get_error_string().str; + constexpr char test_string[] = "testing"; + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ASSERT_STREQ(msg.string_value.data, test_string); + ret = rmw_serialize(&msg, ts, &serialized_msg); + ASSERT_EQ(RMW_RET_OK, ret); + + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ( + RMW_RET_OK, + rmw_serialized_message_fini(&serialized_msg)) << rcl_get_error_string().str; + }); + rcl_reset_error(); + + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); + { + ret = rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + { + rcl_serialized_message_t serialized_msg_rcv = rmw_get_zero_initialized_serialized_message(); + initial_capacity_ser = 0u; + ASSERT_EQ( + RCL_RET_OK, rmw_serialized_message_init( + &serialized_msg_rcv, initial_capacity_ser, &allocator)) << rcl_get_error_string().str; + ret = rcl_take_serialized_message(&subscription, &serialized_msg_rcv, nullptr, nullptr); + ASSERT_EQ(RMW_RET_OK, ret); + + test_msgs__msg__Strings msg_rcv; + test_msgs__msg__Strings__init(&msg_rcv); + ret = rmw_deserialize(&serialized_msg_rcv, ts, &msg_rcv); + ASSERT_EQ(RMW_RET_OK, ret); + ASSERT_EQ( + std::string(test_string), std::string(msg_rcv.string_value.data, msg_rcv.string_value.size)); + + test_msgs__msg__Strings__fini(&msg_rcv); + ASSERT_EQ( + RMW_RET_OK, + rmw_serialized_message_fini(&serialized_msg_rcv)) << rcl_get_error_string().str; + } +} + +/* Basic test for subscription loan functions + */ +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_loaned) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic[] = "rcl_loan"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); + const char * test_string = "testing"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); + { + auto patch_take = + mocking_utils::prepare_patch("lib:rcl", rmw_take_loaned_message_with_info); + auto patch_return = + mocking_utils::prepare_patch("lib:rcl", rmw_return_loaned_message_from_subscription); + + if (!rcl_subscription_can_loan_messages(&subscription)) { + // If rcl (and ultimately rmw) does not support message loaning, + // mock it so that a unit test can still be constructed. + patch_take.then_call( + [](auto sub, void ** loaned_message, auto taken, auto msg_info, auto allocation) { + auto msg = new(std::nothrow) test_msgs__msg__Strings{}; + if (!msg) { + return RMW_RET_BAD_ALLOC; + } + test_msgs__msg__Strings__init(msg); + *loaned_message = static_cast(msg); + rmw_ret_t ret = rmw_take_with_info(sub, *loaned_message, taken, msg_info, allocation); + if (RMW_RET_OK != ret) { + delete msg; + } + return ret; + }); + patch_return.then_call( + [](auto, void * loaned_message) { + auto msg = reinterpret_cast(loaned_message); + test_msgs__msg__Strings__fini(msg); + delete msg; + + return RMW_RET_OK; + }); + } + + test_msgs__msg__Strings * msg_loaned = nullptr; + ret = rcl_take_loaned_message( + &subscription, reinterpret_cast(&msg_loaned), nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ( + std::string(test_string), + std::string(msg_loaned->string_value.data, msg_loaned->string_value.size)); + ret = rcl_return_loaned_message_from_subscription(&subscription, msg_loaned); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +} + +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription_loan_disable) { + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic[] = "pod_msg"; + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_ret_t ret = + rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + if (rcl_subscription_can_loan_messages(&subscription)) { + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "0")); + EXPECT_TRUE(rcl_subscription_can_loan_messages(&subscription)); + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "1")); + EXPECT_FALSE(rcl_subscription_can_loan_messages(&subscription)); + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "2")); + EXPECT_TRUE(rcl_subscription_can_loan_messages(&subscription)); + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "Unexpected")); + EXPECT_TRUE(rcl_subscription_can_loan_messages(&subscription)); + } else { + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "0")); + EXPECT_FALSE(rcl_subscription_can_loan_messages(&subscription)); + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "1")); + EXPECT_FALSE(rcl_subscription_can_loan_messages(&subscription)); + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "2")); + EXPECT_FALSE(rcl_subscription_can_loan_messages(&subscription)); + ASSERT_TRUE(rcutils_set_env("ROS_DISABLE_LOANED_MESSAGES", "Unexpected")); + EXPECT_FALSE(rcl_subscription_can_loan_messages(&subscription)); + } +} + +/* Test for all failure modes in subscription take with loaned messages function. + */ +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_bad_take_loaned_message) { + constexpr char topic[] = "rcl_loan"; + const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rmw_ret_t ret = rcl_subscription_init( + &subscription, this->node_ptr, ts, topic, + &subscription_options); + ASSERT_EQ(RMW_RET_OK, ret) << rcl_get_error_string().str; + + test_msgs__msg__Strings * loaned_message = nullptr; + void ** type_erased_loaned_message_pointer = reinterpret_cast(&loaned_message); + rmw_message_info_t * message_info = nullptr; // is a valid argument + rmw_subscription_allocation_t * allocation = nullptr; // is a valid argument + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, rcl_take_loaned_message( + nullptr, type_erased_loaned_message_pointer, message_info, allocation)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_take_loaned_message(&subscription, nullptr, message_info, allocation)); + rcl_reset_error(); + + test_msgs__msg__Strings dummy_message; + loaned_message = &dummy_message; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_take_loaned_message( + &subscription, type_erased_loaned_message_pointer, message_info, allocation)); + rcl_reset_error(); + loaned_message = nullptr; + + { + rmw_ret_t rmw_take_loaned_message_with_info_returns = RMW_RET_OK; + auto mock = mocking_utils::patch( + "lib:rcl", rmw_take_loaned_message_with_info, + [&](auto, auto, bool * taken, auto && ...) { + *taken = false; + return rmw_take_loaned_message_with_info_returns; + }); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_TAKE_FAILED, rcl_take_loaned_message( + &subscription, type_erased_loaned_message_pointer, message_info, allocation)); + rcl_reset_error(); + + rmw_take_loaned_message_with_info_returns = RMW_RET_BAD_ALLOC; + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_take_loaned_message( + &subscription, type_erased_loaned_message_pointer, message_info, allocation)); + rcl_reset_error(); + + rmw_take_loaned_message_with_info_returns = RMW_RET_UNSUPPORTED; + EXPECT_EQ( + RCL_RET_UNSUPPORTED, rcl_take_loaned_message( + &subscription, type_erased_loaned_message_pointer, message_info, allocation)); + rcl_reset_error(); + + rmw_take_loaned_message_with_info_returns = RMW_RET_ERROR; + EXPECT_EQ( + RCL_RET_ERROR, rcl_take_loaned_message( + &subscription, type_erased_loaned_message_pointer, message_info, allocation)); + rcl_reset_error(); + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_fini(&subscription, this->node_ptr)) << rcl_get_error_string().str; +} + +/* Test for all failure modes in subscription return loaned messages function. + */ +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_bad_return_loaned_message) { + constexpr char topic[] = "rcl_loan"; + const rosidl_message_type_support_t * ts = ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + test_msgs__msg__Strings dummy_message; + test_msgs__msg__Strings__init(&dummy_message); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&dummy_message); + }); + void * loaned_message = &dummy_message; + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_return_loaned_message_from_subscription(nullptr, loaned_message)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_return_loaned_message_from_subscription(&subscription, loaned_message)); + rcl_reset_error(); + + rmw_ret_t ret = rcl_subscription_init( + &subscription, this->node_ptr, ts, topic, + &subscription_options); + ASSERT_EQ(RMW_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_return_loaned_message_from_subscription(&subscription, nullptr)); + rcl_reset_error(); + + { + rmw_ret_t rmw_return_loaned_message_from_subscription_returns = RMW_RET_OK; + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_return_loaned_message_from_subscription, + rmw_return_loaned_message_from_subscription_returns); + + EXPECT_EQ( + RCL_RET_OK, rcl_return_loaned_message_from_subscription(&subscription, loaned_message)) << + rcl_get_error_string().str; + + rmw_return_loaned_message_from_subscription_returns = RMW_RET_UNSUPPORTED; + EXPECT_EQ( + RCL_RET_UNSUPPORTED, rcl_return_loaned_message_from_subscription( + &subscription, + loaned_message)); + rcl_reset_error(); + + rmw_return_loaned_message_from_subscription_returns = RMW_RET_ERROR; + EXPECT_EQ( + RCL_RET_ERROR, rcl_return_loaned_message_from_subscription(&subscription, loaned_message)); + rcl_reset_error(); + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_fini(&subscription, this->node_ptr)) << rcl_get_error_string().str; +} + +/* A subscription with a content filtered topic setting. + */ +TEST_F( + CLASSNAME( + TestSubscriptionFixture, + RMW_IMPLEMENTATION), test_subscription_content_filtered) { + const char * filter_expression1 = "string_value = 'FilteredData'"; + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic[] = "rcl_test_subscription_content_filtered_chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_options_set_content_filter_options( + filter_expression1, 0, nullptr, &subscription_options) + ); + + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + bool is_cft_support = rcl_subscription_is_cft_enabled(&subscription); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 1000)); + + // publish with a non-filtered data + constexpr char test_string[] = "NotFilteredData"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + if (is_cft_support) { + ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + } else { + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ( + std::string(test_string), + std::string(msg.string_value.data, msg.string_value.size)); + } + + constexpr char test_filtered_string[] = "FilteredData"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_filtered_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ( + std::string(test_filtered_string), + std::string(msg.string_value.data, msg.string_value.size)); + } + + // set filter + const char * filter_expression2 = "string_value = %0"; + const char * expression_parameters2[] = {"'FilteredOtherData'"}; + size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); + { + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_init( + &subscription, + filter_expression2, expression_parameters2_count, expression_parameters2, + &options) + ); + + ret = rcl_subscription_set_content_filter( + &subscription, &options); + if (is_cft_support) { + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(10)); + } else { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini( + &subscription, &options) + ); + } + + // publish FilteredData again + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_filtered_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + if (is_cft_support) { + ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + } else { + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ( + std::string(test_filtered_string), + std::string(msg.string_value.data, msg.string_value.size)); + } + + constexpr char test_filtered_other_string[] = "FilteredOtherData"; + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_filtered_other_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ( + std::string(test_filtered_other_string), + std::string(msg.string_value.data, msg.string_value.size)); + } + + // get filter + { + rcl_subscription_content_filter_options_t content_filter_options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + ret = rcl_subscription_get_content_filter( + &subscription, &content_filter_options); + if (is_cft_support) { + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rmw_subscription_content_filter_options_t * options = + &content_filter_options.rmw_subscription_content_filter_options; + ASSERT_STREQ(filter_expression2, options->filter_expression); + ASSERT_EQ(expression_parameters2_count, options->expression_parameters.size); + for (size_t i = 0; i < expression_parameters2_count; ++i) { + EXPECT_STREQ( + options->expression_parameters.data[i], + expression_parameters2[i]); + } + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini( + &subscription, + &content_filter_options) + ); + } else { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + } + } + + // reset filter + { + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_init( + &subscription, + "", 0, nullptr, + &options) + ); + + ret = rcl_subscription_set_content_filter( + &subscription, &options); + if (is_cft_support) { + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(10)); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 1000)); + ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); + } else { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini( + &subscription, &options) + ); + } + + // publish with a non-filtered data again + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + ASSERT_TRUE(rosidl_runtime_c__String__assign(&msg.string_value, test_string)); + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__Strings__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + + { + test_msgs__msg__Strings msg; + test_msgs__msg__Strings__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__Strings__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_EQ( + std::string(test_string), + std::string(msg.string_value.data, msg.string_value.size)); + } +} + +/* A subscription without a content filtered topic setting at beginning. + */ +TEST_F( + CLASSNAME( + TestSubscriptionFixture, + RMW_IMPLEMENTATION), test_subscription_not_initialized_with_content_filtering) { + rcl_ret_t ret; + rcl_publisher_t publisher = rcl_get_zero_initialized_publisher(); + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic[] = "rcl_test_subscription_not_begin_content_filtered_chatter"; + rcl_publisher_options_t publisher_options = rcl_publisher_get_default_options(); + ret = rcl_publisher_init(&publisher, this->node_ptr, ts, topic, &publisher_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_publisher_fini(&publisher, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + // not to set filter expression + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_FALSE(rcl_subscription_is_cft_enabled(&subscription)); + + // failed to get filter + { + rcl_subscription_content_filter_options_t content_filter_options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + ret = rcl_subscription_get_content_filter( + &subscription, &content_filter_options); + ASSERT_NE(RCL_RET_OK, ret); + } + + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 1000)); + + // publish with a non-filtered data + int32_t test_value = 3; + { + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + msg.int32_value = test_value; + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__BasicTypes__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + + { + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__BasicTypes__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_TRUE(test_value == msg.int32_value); + } + + // set filter + const char * filter_expression2 = "int32_value = %0"; + const char * expression_parameters2[] = {"4"}; + size_t expression_parameters2_count = sizeof(expression_parameters2) / sizeof(char *); + bool is_cft_support = + (std::string(rmw_get_implementation_identifier()).find("rmw_connextdds") == 0 || + std::string(rmw_get_implementation_identifier()).find("rmw_fastrtps_cpp") == 0); + { + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_init( + &subscription, + filter_expression2, expression_parameters2_count, expression_parameters2, + &options) + ); + + ret = rcl_subscription_set_content_filter( + &subscription, &options); + if (!is_cft_support) { + ASSERT_EQ(RCL_RET_UNSUPPORTED, ret); + } else { + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // waiting to allow for filter propagation + std::this_thread::sleep_for(std::chrono::seconds(10)); + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini( + &subscription, &options) + ); + } + + // publish no filtered data again + { + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + msg.int32_value = test_value; + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__BasicTypes__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + if (is_cft_support) { + ASSERT_FALSE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + } else { + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__BasicTypes__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_TRUE(test_value == msg.int32_value); + } + + // publish filtered data + int32_t test_filtered_value = 4; + { + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + msg.int32_value = test_filtered_value; + ret = rcl_publish(&publisher, &msg, nullptr); + test_msgs__msg__BasicTypes__fini(&msg); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 1000)); + + { + test_msgs__msg__BasicTypes msg; + test_msgs__msg__BasicTypes__init(&msg); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__BasicTypes__fini(&msg); + }); + ret = rcl_take(&subscription, &msg, nullptr, nullptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ASSERT_TRUE(test_filtered_value == msg.int32_value); + } +} + +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_get_options) { + rcl_ret_t ret; + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, Strings); + constexpr char topic[] = "test_get_options"; + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init(&subscription, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + const rcl_subscription_options_t * get_sub_options = rcl_subscription_get_options(&subscription); + ASSERT_EQ(subscription_options.qos.history, get_sub_options->qos.history); + ASSERT_EQ(subscription_options.qos.depth, get_sub_options->qos.depth); + ASSERT_EQ(subscription_options.qos.durability, get_sub_options->qos.durability); + + ASSERT_EQ(NULL, rcl_subscription_get_options(nullptr)); +} + +/* bad take() +*/ +TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscription_bad_take) { + test_msgs__msg__BasicTypes msg; + rmw_message_info_t message_info = rmw_get_zero_initialized_message_info(); + ASSERT_TRUE(test_msgs__msg__BasicTypes__init(&msg)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + test_msgs__msg__BasicTypes__fini(&msg); + }); + EXPECT_EQ(RCL_RET_SUBSCRIPTION_INVALID, rcl_take(nullptr, &msg, &message_info, nullptr)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, rcl_take(&subscription_zero_init, &msg, &message_info, nullptr)); + rcl_reset_error(); + + rmw_ret_t rmw_take_with_info_returns = RMW_RET_OK; + auto mock = mocking_utils::patch( + "lib:rcl", rmw_take_with_info, + [&](auto, auto, bool * taken, auto...) { + *taken = false; + return rmw_take_with_info_returns; + }); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_TAKE_FAILED, rcl_take(&subscription, &msg, &message_info, nullptr)); + rcl_reset_error(); + + rmw_take_with_info_returns = RMW_RET_BAD_ALLOC; + EXPECT_EQ( + RCL_RET_BAD_ALLOC, rcl_take(&subscription, &msg, &message_info, nullptr)); + rcl_reset_error(); + + rmw_take_with_info_returns = RMW_RET_ERROR; + EXPECT_EQ( + RCL_RET_ERROR, rcl_take(&subscription, &msg, &message_info, nullptr)); + rcl_reset_error(); +} + +/* bad take_serialized +*/ +TEST_F( + CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), + test_subscription_bad_take_serialized) { + rcl_serialized_message_t serialized_msg = rmw_get_zero_initialized_serialized_message(); + size_t initial_serialization_capacity = 0u; + ASSERT_EQ( + RCL_RET_OK, rmw_serialized_message_init( + &serialized_msg, initial_serialization_capacity, &allocator)) << + rcl_get_error_string().str; + + rmw_message_info_t * message_info = nullptr; // is a valid argument + rmw_subscription_allocation_t * allocation = nullptr; // is a valid argument + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_take_serialized_message(nullptr, &serialized_msg, message_info, allocation)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_take_serialized_message( + &subscription_zero_init, &serialized_msg, + message_info, allocation)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_take_serialized_message(&subscription, nullptr, message_info, allocation)); + rcl_reset_error(); + + rmw_ret_t rmw_take_serialized_message_with_info_returns = RMW_RET_OK; + auto mock = mocking_utils::patch( + "lib:rcl", rmw_take_serialized_message_with_info, + [&](auto, auto, bool * taken, auto...) { + *taken = false; + return rmw_take_serialized_message_with_info_returns; + }); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_TAKE_FAILED, + rcl_take_serialized_message(&subscription, &serialized_msg, message_info, allocation)); + rcl_reset_error(); + + rmw_take_serialized_message_with_info_returns = RMW_RET_BAD_ALLOC; + EXPECT_EQ( + RCL_RET_BAD_ALLOC, + rcl_take_serialized_message(&subscription, &serialized_msg, message_info, allocation)); + rcl_reset_error(); + + rmw_take_serialized_message_with_info_returns = RMW_RET_ERROR; + EXPECT_EQ( + RCL_RET_ERROR, + rcl_take_serialized_message(&subscription, &serialized_msg, message_info, allocation)); + rcl_reset_error(); +} + +/* Bad arguments take_sequence + */ +TEST_F( + CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscription_bad_take_sequence) +{ + size_t seq_size = 3u; + rmw_message_sequence_t messages; + ASSERT_EQ(RMW_RET_OK, rmw_message_sequence_init(&messages, seq_size, &allocator)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RMW_RET_OK, rmw_message_sequence_fini(&messages)); + }); + rmw_message_info_sequence_t message_infos_short; + ASSERT_EQ( + RMW_RET_OK, rmw_message_info_sequence_init(&message_infos_short, seq_size - 1u, &allocator)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RMW_RET_OK, rmw_message_info_sequence_fini(&message_infos_short)); + }); + rmw_message_info_sequence_t message_infos; + ASSERT_EQ( + RMW_RET_OK, rmw_message_info_sequence_init(&message_infos, seq_size, &allocator)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RMW_RET_OK, rmw_message_info_sequence_fini(&message_infos)); + }); + rmw_subscription_allocation_t * allocation = nullptr; // is a valid argument + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_take_sequence(nullptr, seq_size, &messages, &message_infos, allocation)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_take_sequence(&subscription_zero_init, seq_size, &messages, &message_infos, allocation)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_take_sequence(&subscription, seq_size + 1, &messages, &message_infos, allocation)); + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_take_sequence(&subscription, seq_size, &messages, &message_infos_short, allocation)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_take_sequence(&subscription, seq_size, nullptr, &message_infos, allocation)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_take_sequence(&subscription, seq_size, &messages, nullptr, allocation)); + rcl_reset_error(); + + rmw_ret_t rmw_take_sequence_returns = RMW_RET_OK; + auto mock = mocking_utils::patch( + "lib:rcl", rmw_take_sequence, + [&](auto, auto, auto, auto, size_t * taken, auto) { + *taken = 0u; + return rmw_take_sequence_returns; + }); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_TAKE_FAILED, + rcl_take_sequence(&subscription, seq_size, &messages, &message_infos, allocation)); + rcl_reset_error(); + + rmw_take_sequence_returns = RMW_RET_BAD_ALLOC; + EXPECT_EQ( + RCL_RET_BAD_ALLOC, + rcl_take_sequence(&subscription, seq_size, &messages, &message_infos, allocation)); + rcl_reset_error(); + + rmw_take_sequence_returns = RMW_RET_ERROR; + EXPECT_EQ( + RCL_RET_ERROR, + rcl_take_sequence(&subscription, seq_size, &messages, &message_infos, allocation)); + rcl_reset_error(); +} + +/* Test for all failure modes in subscription get_publisher_count function. + */ +TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_bad_get_publisher_count) { + size_t publisher_count = 0; + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_get_publisher_count(nullptr, &publisher_count)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_get_publisher_count(&subscription_zero_init, &publisher_count)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_get_publisher_count(&subscription, nullptr)); + rcl_reset_error(); + + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_count_matched_publishers, RMW_RET_ERROR); + EXPECT_EQ( + RCL_RET_ERROR, + rcl_subscription_get_publisher_count(&subscription, &publisher_count)); + rcl_reset_error(); +} + +/* Using bad arguments subscription methods + */ +TEST_F(CLASSNAME(TestSubscriptionFixtureInit, RMW_IMPLEMENTATION), test_subscription_bad_argument) { + EXPECT_EQ(NULL, rcl_subscription_get_actual_qos(nullptr)); + rcl_reset_error(); + EXPECT_FALSE(rcl_subscription_can_loan_messages(nullptr)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_rmw_handle(nullptr)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_topic_name(nullptr)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_options(nullptr)); + rcl_reset_error(); + EXPECT_FALSE(rcl_subscription_is_cft_enabled(nullptr)); + rcl_reset_error(); + + EXPECT_EQ(NULL, rcl_subscription_get_actual_qos(&subscription_zero_init)); + rcl_reset_error(); + EXPECT_FALSE(rcl_subscription_can_loan_messages(&subscription_zero_init)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_rmw_handle(&subscription_zero_init)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_topic_name(&subscription_zero_init)); + rcl_reset_error(); + EXPECT_EQ(NULL, rcl_subscription_get_options(&subscription_zero_init)); + rcl_reset_error(); + EXPECT_FALSE(rcl_subscription_is_cft_enabled(&subscription_zero_init)); + rcl_reset_error(); +} + +/* Test for all failure modes in rcl_subscription_set_content_filter function. + */ +TEST_F( + CLASSNAME( + TestSubscriptionFixtureInit, + RMW_IMPLEMENTATION), test_bad_rcl_subscription_set_content_filter) { + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_set_content_filter(nullptr, nullptr)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_set_content_filter(&subscription_zero_init, nullptr)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_set_content_filter(&subscription, nullptr)); + rcl_reset_error(); + + // an options used later + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_init( + &subscription, + "data = '0'", + 0, + nullptr, + &options + ) + ); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini(&subscription, &options) + ); + }); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_UNSUPPORTED); + EXPECT_EQ( + RMW_RET_UNSUPPORTED, + rcl_subscription_set_content_filter( + &subscription, &options)); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_set_content_filter, RMW_RET_ERROR); + EXPECT_EQ( + RMW_RET_ERROR, + rcl_subscription_set_content_filter( + &subscription, &options)); + rcl_reset_error(); + } +} + +/* Test for all failure modes in rcl_subscription_get_content_filter function. + */ +TEST_F( + CLASSNAME( + TestSubscriptionFixtureInit, + RMW_IMPLEMENTATION), test_bad_rcl_subscription_get_content_filter) { + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_get_content_filter(nullptr, nullptr)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_get_content_filter(&subscription_zero_init, nullptr)); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_get_content_filter(&subscription, nullptr)); + rcl_reset_error(); + + rcl_subscription_content_filter_options_t options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_get_content_filter, RMW_RET_UNSUPPORTED); + EXPECT_EQ( + RMW_RET_UNSUPPORTED, + rcl_subscription_get_content_filter( + &subscription, &options)); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_subscription_get_content_filter, RMW_RET_ERROR); + EXPECT_EQ( + RMW_RET_ERROR, + rcl_subscription_get_content_filter( + &subscription, &options)); + rcl_reset_error(); + } +} + +TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_init_fini_maybe_fail) +{ + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic[] = "chatter"; + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + rcl_subscription_t subscription = rcl_get_zero_initialized_subscription(); + + RCUTILS_FAULT_INJECTION_TEST( + { + rcl_ret_t ret = rcl_subscription_init( + &subscription, this->node_ptr, ts, topic, &subscription_options); + + if (RCL_RET_OK == ret) { + EXPECT_TRUE(rcl_subscription_is_valid(&subscription)); + ret = rcl_subscription_fini(&subscription, this->node_ptr); + if (RCL_RET_OK != ret) { + // If fault injection caused fini to fail, we should try it again. + EXPECT_EQ(RCL_RET_OK, rcl_subscription_fini(&subscription, this->node_ptr)); + } + } else { + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + }); +} diff --git a/test/rcl/test_subscription_content_filter_options.cpp b/test/rcl/test_subscription_content_filter_options.cpp new file mode 100644 index 0000000..163bf3d --- /dev/null +++ b/test/rcl/test_subscription_content_filter_options.cpp @@ -0,0 +1,354 @@ +// 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 + +#include "rcl/error_handling.h" +#include "rcl/node.h" +#include "rcl/rcl.h" +#include "rcl/subscription.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "test_msgs/msg/basic_types.h" + + +TEST(TestSubscriptionOptionsContentFilter, subscription_options_failure) { + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + + const char * filter_expression1 = "filter=1"; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_options_set_content_filter_options( + nullptr, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_options_set_content_filter_options( + filter_expression1, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_options_set_content_filter_options( + filter_expression1, 1, nullptr, &subscription_options) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_options_fini( + nullptr) + ); + rcl_reset_error(); +} + +TEST(TestSubscriptionOptionsContentFilter, subscription_options_success) +{ + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + + const char * filter_expression1 = "filter=1"; + + { + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_options_set_content_filter_options( + filter_expression1, 0, nullptr, &subscription_options) + ); + + rmw_subscription_content_filter_options_t * content_filter_options = + subscription_options.rmw_subscription_options.content_filter_options; + ASSERT_NE(nullptr, content_filter_options); + EXPECT_STREQ(filter_expression1, content_filter_options->filter_expression); + EXPECT_EQ(0u, content_filter_options->expression_parameters.size); + EXPECT_EQ(nullptr, content_filter_options->expression_parameters.data); + } + + const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2"; + const char * expression_parameters2[] = { + "1", "2", "3", + }; + size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char *); + + { + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_options_set_content_filter_options( + filter_expression2, expression_parameters_count2, + expression_parameters2, &subscription_options) + ); + + rmw_subscription_content_filter_options_t * content_filter_options = + subscription_options.rmw_subscription_options.content_filter_options; + ASSERT_NE(nullptr, content_filter_options); + EXPECT_STREQ(filter_expression2, content_filter_options->filter_expression); + ASSERT_EQ( + expression_parameters_count2, + content_filter_options->expression_parameters.size); + for (size_t i = 0; i < expression_parameters_count2; ++i) { + EXPECT_STREQ( + content_filter_options->expression_parameters.data[i], + expression_parameters2[i]); + } + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_options_fini(&subscription_options) + ); +} + + +class TestSubscriptionContentFilterOptions : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + rcl_subscription_t * subscription_ptr; + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + constexpr char name[] = "test_subscription_content_filter_options_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + const rosidl_message_type_support_t * ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes); + constexpr char topic[] = "chatter"; + + this->subscription_ptr = new rcl_subscription_t; + *this->subscription_ptr = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t subscription_options = rcl_subscription_get_default_options(); + ret = rcl_subscription_init( + this->subscription_ptr, this->node_ptr, ts, topic, &subscription_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret = rcl_subscription_fini(this->subscription_ptr, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +TEST_F(TestSubscriptionContentFilterOptions, content_filter_options_failure) { + rcl_subscription_content_filter_options_t content_filter_options = + rcl_get_zero_initialized_subscription_content_filter_options(); + + const char * filter_expression1 = "filter=1"; + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_content_filter_options_init( + nullptr, nullptr, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_init( + this->subscription_ptr, nullptr, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_init( + this->subscription_ptr, filter_expression1, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_init( + this->subscription_ptr, filter_expression1, 1, nullptr, &content_filter_options) + ); + rcl_reset_error(); + + // set + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_content_filter_options_set( + nullptr, nullptr, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_set( + this->subscription_ptr, nullptr, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_set( + this->subscription_ptr, filter_expression1, 0, nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_set( + this->subscription_ptr, filter_expression1, 1, nullptr, &content_filter_options) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_SUBSCRIPTION_INVALID, + rcl_subscription_content_filter_options_fini( + nullptr, nullptr) + ); + rcl_reset_error(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_subscription_content_filter_options_fini( + this->subscription_ptr, nullptr) + ); + rcl_reset_error(); +} + +TEST_F(TestSubscriptionContentFilterOptions, content_filter_options_success) +{ + rmw_subscription_content_filter_options_t * content_filter_options; + const char * filter_expression1 = "filter=1"; + const char * filter_expression1_update = "filter=2"; + + rcl_subscription_content_filter_options_t subscription_content_filter_options = + rcl_get_zero_initialized_subscription_content_filter_options(); + { + // init with filter_expression1 + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_init( + this->subscription_ptr, filter_expression1, 0, nullptr, + &subscription_content_filter_options) + ); + + content_filter_options = + &subscription_content_filter_options.rmw_subscription_content_filter_options; + EXPECT_STREQ(filter_expression1, content_filter_options->filter_expression); + EXPECT_EQ(0u, content_filter_options->expression_parameters.size); + EXPECT_EQ(nullptr, content_filter_options->expression_parameters.data); + + // set with filter_expression1_update + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_set( + this->subscription_ptr, filter_expression1_update, 0, nullptr, + &subscription_content_filter_options) + ); + + content_filter_options = + &subscription_content_filter_options.rmw_subscription_content_filter_options; + EXPECT_STREQ(filter_expression1_update, content_filter_options->filter_expression); + EXPECT_EQ(0u, content_filter_options->expression_parameters.size); + EXPECT_EQ(nullptr, content_filter_options->expression_parameters.data); + } + + const char * filter_expression2 = "(filter1=%0 OR filter1=%1) AND filter2=%2"; + const char * expression_parameters2[] = { + "1", "2", "3", + }; + size_t expression_parameters_count2 = sizeof(expression_parameters2) / sizeof(char *); + + const char * filter_expression2_update = "(filter1=%0 AND filter1=%1) OR filter2=%2"; + const char * expression_parameters2_update[] = { + "11", "22", "33", + }; + size_t expression_parameters_count2_update = sizeof(expression_parameters2) / sizeof(char *); + + rcl_subscription_content_filter_options_t subscription_content_filter_options2 = + rcl_get_zero_initialized_subscription_content_filter_options(); + { + // init with filter_expression2 and expression_parameters2 + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_init( + this->subscription_ptr, filter_expression2, expression_parameters_count2, + expression_parameters2, &subscription_content_filter_options2) + ); + + content_filter_options = + &subscription_content_filter_options2.rmw_subscription_content_filter_options; + ASSERT_NE(nullptr, content_filter_options); + EXPECT_STREQ(filter_expression2, content_filter_options->filter_expression); + ASSERT_EQ( + expression_parameters_count2, + content_filter_options->expression_parameters.size); + for (size_t i = 0; i < expression_parameters_count2; ++i) { + EXPECT_STREQ( + content_filter_options->expression_parameters.data[i], + expression_parameters2[i]); + } + + // set with filter_expression2_update and expression_parameters2_update + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_set( + this->subscription_ptr, filter_expression2_update, expression_parameters_count2_update, + expression_parameters2_update, &subscription_content_filter_options2) + ); + + content_filter_options = + &subscription_content_filter_options2.rmw_subscription_content_filter_options; + ASSERT_NE(nullptr, content_filter_options); + EXPECT_STREQ(filter_expression2_update, content_filter_options->filter_expression); + ASSERT_EQ( + expression_parameters_count2_update, + content_filter_options->expression_parameters.size); + for (size_t i = 0; i < expression_parameters_count2_update; ++i) { + EXPECT_STREQ( + content_filter_options->expression_parameters.data[i], + expression_parameters2_update[i]); + } + } + + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini( + this->subscription_ptr, &subscription_content_filter_options) + ); + EXPECT_EQ( + RCL_RET_OK, + rcl_subscription_content_filter_options_fini( + this->subscription_ptr, &subscription_content_filter_options2) + ); +} diff --git a/test/rcl/test_time.cpp b/test/rcl/test_time.cpp new file mode 100644 index 0000000..60e4b56 --- /dev/null +++ b/test/rcl/test_time.cpp @@ -0,0 +1,801 @@ +// Copyright 2015 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 "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp" +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rcl/time.h" + +#include "./allocator_testing_utils.h" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +using osrf_testing_tools_cpp::memory_tools::on_unexpected_malloc; +using osrf_testing_tools_cpp::memory_tools::on_unexpected_realloc; +using osrf_testing_tools_cpp::memory_tools::on_unexpected_calloc; +using osrf_testing_tools_cpp::memory_tools::on_unexpected_free; + +class CLASSNAME (TestTimeFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + void SetUp() + { + osrf_testing_tools_cpp::memory_tools::initialize(); + on_unexpected_malloc([]() {ADD_FAILURE() << "UNEXPECTED MALLOC";}); + on_unexpected_realloc([]() {ADD_FAILURE() << "UNEXPECTED REALLOC";}); + on_unexpected_calloc([]() {ADD_FAILURE() << "UNEXPECTED CALLOC";}); + on_unexpected_free([]() {ADD_FAILURE() << "UNEXPECTED FREE";}); + } + + void TearDown() + { + osrf_testing_tools_cpp::memory_tools::uninitialize(); + } +}; + +// Tests the rcl_set_ros_time_override() function. +TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_ros_time_set_override) { + osrf_testing_tools_cpp::memory_tools::enable_monitoring_in_all_threads(); + rcl_clock_t ros_clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t retval = rcl_ros_clock_init(&ros_clock, &allocator); + ASSERT_EQ(RCL_RET_OK, retval) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)) << rcl_get_error_string().str; + }); + + rcl_ret_t ret; + // Check for invalid argument error condition (allowed to alloc). + ret = rcl_set_ros_time_override(nullptr, 0); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + bool result; + ret = rcl_is_enabled_ros_time_override(nullptr, &result); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_is_enabled_ros_time_override(&ros_clock, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_is_enabled_ros_time_override(nullptr, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + rcl_time_point_value_t query_now; + bool is_enabled; + ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(is_enabled, false); + EXPECT_NO_MEMORY_OPERATIONS( + { + // Check for normal operation (not allowed to alloc). + ret = rcl_clock_get_now(&ros_clock, &query_now); + }); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_NE(query_now, 0u); + // Compare to std::chrono::system_clock time (within a second). + ret = rcl_clock_get_now(&ros_clock, &query_now); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + { + std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now(); + auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); + int64_t now_ns_int = now_ns.count(); + int64_t now_diff = query_now - now_ns_int; + const int k_tolerance_ms = 1000; + EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs"; + } + // Test ros time specific APIs + rcl_time_point_value_t set_point = 1000000000ull; + // Check initialized state + ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(is_enabled, false); + // set the time point + ret = rcl_set_ros_time_override(&ros_clock, set_point); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + // check still disabled + ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(is_enabled, false); + // get real + ret = rcl_clock_get_now(&ros_clock, &query_now); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + { + std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now(); + auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); + int64_t now_ns_int = now_ns.count(); + int64_t now_diff = query_now - now_ns_int; + const int k_tolerance_ms = 1000; + EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs"; + } + // enable + ret = rcl_enable_ros_time_override(&ros_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + // check enabled + ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(is_enabled, true); + // get sim + ret = rcl_clock_get_now(&ros_clock, &query_now); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(query_now, set_point); + // disable + ret = rcl_disable_ros_time_override(&ros_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + // check disabled + ret = rcl_is_enabled_ros_time_override(&ros_clock, &is_enabled); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(is_enabled, false); + // get real + ret = rcl_clock_get_now(&ros_clock, &query_now); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + { + std::chrono::system_clock::time_point now_sc = std::chrono::system_clock::now(); + auto now_ns = std::chrono::duration_cast(now_sc.time_since_epoch()); + int64_t now_ns_int = now_ns.count(); + int64_t now_diff = query_now - now_ns_int; + const int k_tolerance_ms = 1000; + EXPECT_LE(llabs(now_diff), RCL_MS_TO_NS(k_tolerance_ms)) << "ros_clock differs"; + } +} + +TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_rcl_init_for_clock_and_point) { + rcl_ret_t ret; + rcl_allocator_t allocator = rcl_get_default_allocator(); + // Check for invalid argument error condition (allowed to alloc). + ret = rcl_ros_clock_init(nullptr, &allocator); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + // Check for invalid argument error condition (allowed to alloc). + rcl_clock_t uninitialized_clock; + ret = rcl_ros_clock_init(&uninitialized_clock, nullptr); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + // Check for normal operation (not allowed to alloc). + rcl_clock_t source; + ret = rcl_ros_clock_init(&source, &allocator); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&source)) << rcl_get_error_string().str; + }); + + rcl_clock_t ros_clock; + rcl_ret_t retval = rcl_ros_clock_init(&ros_clock, &allocator); + ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)) << rcl_get_error_string().str; + }); +} + +TEST_F(CLASSNAME(TestTimeFixture, RMW_IMPLEMENTATION), test_ros_clock_initially_zero) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_clock_t ros_clock; + ASSERT_EQ(RCL_RET_OK, rcl_ros_clock_init(&ros_clock, &allocator)) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&ros_clock)) << rcl_get_error_string().str; + }); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&ros_clock)) << rcl_get_error_string().str; + rcl_time_point_value_t query_now = 5; + ASSERT_EQ(RCL_RET_OK, rcl_clock_get_now(&ros_clock, &query_now)) << rcl_get_error_string().str; + EXPECT_EQ(0, query_now); +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), clock_validation) { + ASSERT_FALSE(rcl_clock_valid(NULL)); + rcl_clock_t uninitialized; + // Not reliably detectable due to random values. + // ASSERT_FALSE(rcl_clock_valid(&uninitialized)); + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret; + ret = rcl_ros_clock_init(&uninitialized, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&uninitialized)) << rcl_get_error_string().str; + }); +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), default_clock_instanciation) { + rcl_clock_t ros_clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t retval = rcl_ros_clock_init(&ros_clock, &allocator); + ASSERT_EQ(retval, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)) << rcl_get_error_string().str; + }); + ASSERT_TRUE(rcl_clock_valid(&ros_clock)); + + auto * steady_clock = static_cast( + allocator.zero_allocate(1, sizeof(rcl_clock_t), allocator.state)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + allocator.deallocate(steady_clock, allocator.state); + }); + + retval = rcl_steady_clock_init(steady_clock, &allocator); + EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_steady_clock_fini(steady_clock)) << rcl_get_error_string().str; + }); + ASSERT_TRUE(rcl_clock_valid(steady_clock)); + + auto * system_clock = static_cast( + allocator.zero_allocate(1, sizeof(rcl_clock_t), allocator.state)); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + allocator.deallocate(system_clock, allocator.state); + }); + retval = rcl_system_clock_init(system_clock, &allocator); + EXPECT_EQ(retval, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_system_clock_fini(system_clock)) << rcl_get_error_string().str; + }); + ASSERT_TRUE(rcl_clock_valid(system_clock)); +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), specific_clock_instantiation) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + { + rcl_clock_t uninitialized_clock; + rcl_ret_t ret = rcl_clock_init(RCL_CLOCK_UNINITIALIZED, &uninitialized_clock, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(uninitialized_clock.type, RCL_CLOCK_UNINITIALIZED) << + "Expected time source of type RCL_CLOCK_UNINITIALIZED"; + EXPECT_TRUE(rcutils_allocator_is_valid(&(uninitialized_clock.allocator))); + ret = rcl_clock_fini(&uninitialized_clock); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ( + rcl_ros_clock_fini(&uninitialized_clock), RCL_RET_ERROR) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ( + rcl_steady_clock_fini(&uninitialized_clock), RCL_RET_ERROR) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ( + rcl_system_clock_fini(&uninitialized_clock), RCL_RET_ERROR) << rcl_get_error_string().str; + rcl_reset_error(); + } + { + rcl_clock_t ros_clock; + rcl_ret_t ret = rcl_clock_init(RCL_ROS_TIME, &ros_clock, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(ros_clock.type, RCL_ROS_TIME) << + "Expected time source of type RCL_ROS_TIME"; + ret = rcl_clock_fini(&ros_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + { + rcl_clock_t system_clock; + rcl_ret_t ret = rcl_clock_init(RCL_SYSTEM_TIME, &system_clock, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(system_clock.type, RCL_SYSTEM_TIME) << + "Expected time source of type RCL_SYSTEM_TIME"; + ret = rcl_clock_fini(&system_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + { + rcl_clock_t steady_clock; + rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &steady_clock, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(steady_clock.type, RCL_STEADY_TIME) << + "Expected time source of type RCL_STEADY_TIME"; + ret = rcl_clock_fini(&steady_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + } + { + rcl_clock_t fail_clock; + rcl_clock_type_t undefined_type = (rcl_clock_type_t) 130; + rcl_ret_t ret = rcl_clock_init(undefined_type, &fail_clock, &allocator); + EXPECT_EQ(ret, RCL_RET_INVALID_ARGUMENT) << rcl_get_error_string().str; + } +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference) { + rcl_ret_t ret; + rcl_time_point_t a, b; + + a.nanoseconds = 1000; + b.nanoseconds = 2000; + a.clock_type = RCL_ROS_TIME; + b.clock_type = RCL_ROS_TIME; + + rcl_duration_t d; + ret = rcl_difference_times(&a, &b, &d); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + + EXPECT_EQ(d.nanoseconds, 1000); + + ret = rcl_difference_times(&b, &a, &d); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(d.nanoseconds, -1000); + + b.clock_type = RCL_SYSTEM_TIME; + EXPECT_EQ(rcl_difference_times(&a, &b, &d), RCL_RET_ERROR) << rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_difference_signed) { + rcl_time_point_t a, b; + a.nanoseconds = RCL_S_TO_NS(0LL) + 0LL; + b.nanoseconds = RCL_S_TO_NS(10LL) + 0LL; + a.clock_type = RCL_ROS_TIME; + b.clock_type = RCL_ROS_TIME; + + { + rcl_duration_t d; + rcl_ret_t ret; + ret = rcl_difference_times(&a, &b, &d); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(d.nanoseconds, RCL_S_TO_NS(10LL)); + } + + { + rcl_duration_t d; + rcl_ret_t ret; + ret = rcl_difference_times(&b, &a, &d); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(d.nanoseconds, RCL_S_TO_NS(-10LL)); + } + + // Construct example from issue. + a.nanoseconds = RCL_S_TO_NS(1514423496LL) + 0LL; + b.nanoseconds = RCL_S_TO_NS(1514423498LL) + 147483647LL; + + { + rcl_duration_t d; + rcl_ret_t ret; + ret = rcl_difference_times(&a, &b, &d); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(d.nanoseconds, 2147483647LL); + } + + { + rcl_duration_t d; + rcl_ret_t ret; + ret = rcl_difference_times(&b, &a, &d); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + // The erroneous value was -2147483648 (https://github.com/ros2/rcl/issues/204) + EXPECT_EQ(d.nanoseconds, -2147483647LL); + } +} + +static bool pre_callback_called = false; +static bool post_callback_called = false; + +void clock_callback( + const rcl_time_jump_t * time_jump, + bool before_jump, + void * user_data) +{ + if (before_jump) { + pre_callback_called = true; + EXPECT_FALSE(post_callback_called); + } else { + EXPECT_TRUE(pre_callback_called); + post_callback_called = true; + } + *(static_cast(user_data)) = *time_jump; +} + +void reset_callback_triggers(void) +{ + pre_callback_called = false; + post_callback_called = false; +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_clock_change_callbacks) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_clock_t ros_clock; + rcl_ret_t ret = rcl_ros_clock_init(&ros_clock, &allocator); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)); + }); + rcl_time_point_value_t query_now; + + // set callbacks + rcl_time_jump_t time_jump; + rcl_jump_threshold_t threshold; + threshold.on_clock_change = true; + threshold.min_forward.nanoseconds = 0; + threshold.min_backward.nanoseconds = 0; + ASSERT_EQ( + RCL_RET_OK, + rcl_clock_add_jump_callback(&ros_clock, threshold, clock_callback, &time_jump)) << + rcl_get_error_string().str; + reset_callback_triggers(); + + // Query time, no changes expected. + ret = rcl_clock_get_now(&ros_clock, &query_now); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); + + // Clock change callback called when ROS time is enabled + ret = rcl_enable_ros_time_override(&ros_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_TRUE(pre_callback_called); + EXPECT_TRUE(post_callback_called); + EXPECT_EQ(RCL_ROS_TIME_ACTIVATED, time_jump.clock_change); + reset_callback_triggers(); + + // Clock change callback not called because ROS time is already enabled. + ret = rcl_enable_ros_time_override(&ros_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); + reset_callback_triggers(); + + // Clock change callback called when ROS time is disabled + ret = rcl_disable_ros_time_override(&ros_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_TRUE(pre_callback_called); + EXPECT_TRUE(post_callback_called); + EXPECT_EQ(RCL_ROS_TIME_DEACTIVATED, time_jump.clock_change); + reset_callback_triggers(); + + // Clock change callback not called because ROS time is already disabled. + ret = rcl_disable_ros_time_override(&ros_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); + reset_callback_triggers(); +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_fail_set_jump_callbacks) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_clock_t fail_clock; + rcl_time_jump_t time_jump; + rcl_jump_threshold_t threshold; + rcl_ret_t ret = rcl_clock_init(RCL_CLOCK_UNINITIALIZED, &fail_clock, &allocator); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + threshold.on_clock_change = false; + threshold.min_forward.nanoseconds = -1; + threshold.min_backward.nanoseconds = 0; + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_clock_add_jump_callback(&fail_clock, threshold, clock_callback, &time_jump)) << + rcl_get_error_string().str; + rcl_reset_error(); + + threshold.min_forward.nanoseconds = 0; + threshold.min_backward.nanoseconds = 1; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_clock_add_jump_callback(&fail_clock, threshold, clock_callback, &time_jump)) << + rcl_get_error_string().str; + rcl_reset_error(); +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_forward_jump_callbacks) { + rcl_clock_t ros_clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_ros_clock_init(&ros_clock, &allocator); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)); + }); + + rcl_time_point_value_t set_point1 = 1000L * 1000L * 1000L; + rcl_time_point_value_t set_point2 = 2L * 1000L * 1000L * 1000L; + + rcl_time_jump_t time_jump; + rcl_jump_threshold_t threshold; + threshold.on_clock_change = false; + threshold.min_forward.nanoseconds = 1; + threshold.min_backward.nanoseconds = 0; + ASSERT_EQ( + RCL_RET_OK, + rcl_clock_add_jump_callback(&ros_clock, threshold, clock_callback, &time_jump)) << + rcl_get_error_string().str; + reset_callback_triggers(); + + // Set the time before it's enabled. Should be no callbacks + ret = rcl_set_ros_time_override(&ros_clock, set_point1); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); + + // enable no callbacks + ret = rcl_enable_ros_time_override(&ros_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); + + // Set the time now that it's enabled, now get callbacks + ret = rcl_set_ros_time_override(&ros_clock, set_point2); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_TRUE(pre_callback_called); + EXPECT_TRUE(post_callback_called); + EXPECT_EQ(set_point2 - set_point1, time_jump.delta.nanoseconds); + EXPECT_EQ(RCL_ROS_TIME_NO_CHANGE, time_jump.clock_change); + reset_callback_triggers(); + + // Setting same value as previous time, not a jump + ret = rcl_set_ros_time_override(&ros_clock, set_point2); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); + + // Jump backwards, no jump callbacks + ret = rcl_set_ros_time_override(&ros_clock, set_point1); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); + + // disable no callbacks + ret = rcl_disable_ros_time_override(&ros_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_time_backward_jump_callbacks) { + rcl_clock_t ros_clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_ros_clock_init(&ros_clock, &allocator); + ASSERT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&ros_clock)); + }); + rcl_time_point_value_t set_point1 = 1000000000; + rcl_time_point_value_t set_point2 = 2000000000; + + rcl_time_jump_t time_jump; + rcl_jump_threshold_t threshold; + threshold.on_clock_change = false; + threshold.min_forward.nanoseconds = 0; + threshold.min_backward.nanoseconds = -1; + ASSERT_EQ( + RCL_RET_OK, + rcl_clock_add_jump_callback(&ros_clock, threshold, clock_callback, &time_jump)) << + rcl_get_error_string().str; + reset_callback_triggers(); + + // Set the time before it's enabled. Should be no callbacks + ret = rcl_set_ros_time_override(&ros_clock, set_point2); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); + + // enable no callbacks + ret = rcl_enable_ros_time_override(&ros_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); + + // Set the time now that it's enabled, now get callbacks + ret = rcl_set_ros_time_override(&ros_clock, set_point1); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_TRUE(pre_callback_called); + EXPECT_TRUE(post_callback_called); + EXPECT_EQ(set_point1 - set_point2, time_jump.delta.nanoseconds); + EXPECT_EQ(RCL_ROS_TIME_NO_CHANGE, time_jump.clock_change); + reset_callback_triggers(); + + // Setting same value as previous time, not a jump + ret = rcl_set_ros_time_override(&ros_clock, set_point1); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); + + // Jump forwards, no jump callbacks + ret = rcl_set_ros_time_override(&ros_clock, set_point2); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); + + // disable no callbacks + ret = rcl_disable_ros_time_override(&ros_clock); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_FALSE(pre_callback_called); + EXPECT_FALSE(post_callback_called); +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_clock_add_jump_callback) { + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_ros_clock_init(&clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&clock)); + }); + + rcl_jump_threshold_t threshold; + threshold.on_clock_change = false; + threshold.min_forward.nanoseconds = 0; + threshold.min_backward.nanoseconds = 0; + rcl_jump_callback_t cb = reinterpret_cast(0xBEEF); + void * user_data = reinterpret_cast(0xCAFE); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_clock_add_jump_callback(&clock, threshold, NULL, user_data)); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, NULL)) << + rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_ERROR, rcl_clock_add_jump_callback(&clock, threshold, cb, NULL)); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data)) << + rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_ERROR, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data)); + rcl_reset_error(); + + EXPECT_EQ(2u, clock.num_jump_callbacks); +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), rcl_clock_remove_jump_callback) { + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_ros_clock_init(&clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_ros_clock_fini(&clock)); + }); + + rcl_jump_threshold_t threshold; + threshold.on_clock_change = false; + threshold.min_forward.nanoseconds = 0; + threshold.min_backward.nanoseconds = 0; + rcl_jump_callback_t cb = reinterpret_cast(0xBEEF); + void * user_data1 = reinterpret_cast(0xCAFE); + void * user_data2 = reinterpret_cast(0xFACE); + void * user_data3 = reinterpret_cast(0xBEAD); + void * user_data4 = reinterpret_cast(0xDEED); + + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_clock_remove_jump_callback(&clock, NULL, user_data1)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_clock_remove_jump_callback(&clock, cb, NULL)); + rcl_reset_error(); + + ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data1)) << + rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data2)) << + rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data3)) << + rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data4)) << + rcl_get_error_string().str; + EXPECT_EQ(4u, clock.num_jump_callbacks); + + EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(&clock, cb, user_data3)); + EXPECT_EQ(3u, clock.num_jump_callbacks); + EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(&clock, cb, user_data4)); + EXPECT_EQ(2u, clock.num_jump_callbacks); + EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(&clock, cb, user_data1)); + EXPECT_EQ(1u, clock.num_jump_callbacks); + EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(&clock, cb, user_data2)); + EXPECT_EQ(0u, clock.num_jump_callbacks); +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), add_remove_add_jump_callback) { + rcl_allocator_t failing_allocator = get_failing_allocator(); + set_failing_allocator_is_failing(failing_allocator, false); + + rcl_clock_t clock; + rcl_ret_t ret = rcl_clock_init(RCL_SYSTEM_TIME, &clock, &failing_allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)); + }); + + rcl_jump_threshold_t threshold; + threshold.on_clock_change = false; + threshold.min_forward.nanoseconds = 0; + threshold.min_backward.nanoseconds = 0; + rcl_jump_callback_t cb = reinterpret_cast(0xBEEF); + void * user_data1 = reinterpret_cast(0xCAFE); + void * user_data2 = reinterpret_cast(0xFACE); + void * user_data3 = reinterpret_cast(0xDEED); + + ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data1)) << + rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data2)) << + rcl_get_error_string().str; + EXPECT_EQ(2u, clock.num_jump_callbacks); + + set_failing_allocator_is_failing(failing_allocator, true); + + // Fail to add callback + EXPECT_EQ(RCL_RET_BAD_ALLOC, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data3)); + rcl_reset_error(); + + // Remove callback but fail to shrink storage + EXPECT_EQ(RCL_RET_BAD_ALLOC, rcl_clock_remove_jump_callback(&clock, cb, user_data1)); + rcl_reset_error(); + + set_failing_allocator_is_failing(failing_allocator, false); + + // Callback has already been removed + EXPECT_EQ(RCL_RET_ERROR, rcl_clock_remove_jump_callback(&clock, cb, user_data1)); + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_OK, rcl_clock_remove_jump_callback(&clock, cb, user_data2)); + EXPECT_EQ(0u, clock.num_jump_callbacks); + + EXPECT_EQ(RCL_RET_OK, rcl_clock_add_jump_callback(&clock, threshold, cb, user_data1)) << + rcl_get_error_string().str; + EXPECT_EQ(1u, clock.num_jump_callbacks); +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), failed_get_now) { + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_clock_t uninitialized_clock; + rcl_time_point_value_t query_now; + rcl_ret_t ret = rcl_clock_init(RCL_CLOCK_UNINITIALIZED, &uninitialized_clock, &allocator); + EXPECT_EQ(ret, RCL_RET_OK) << rcl_get_error_string().str; + EXPECT_EQ(uninitialized_clock.type, RCL_CLOCK_UNINITIALIZED); + uninitialized_clock.get_now = NULL; + EXPECT_EQ(RCL_RET_ERROR, rcl_clock_get_now(&uninitialized_clock, &query_now)); + rcl_reset_error(); +} + +TEST(CLASSNAME(rcl_time, RMW_IMPLEMENTATION), fail_ros_time_override) { + bool result; + rcl_time_point_value_t set_point = 1000000000ull; + + rcl_clock_t ros_clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_clock_init(RCL_CLOCK_UNINITIALIZED, &ros_clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_EQ(RCL_RET_ERROR, rcl_enable_ros_time_override(&ros_clock)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_disable_ros_time_override(&ros_clock)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_is_enabled_ros_time_override(&ros_clock, &result)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_set_ros_time_override(&ros_clock, set_point)); + rcl_reset_error(); + + ret = rcl_clock_init(RCL_ROS_TIME, &ros_clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_clock_fini(&ros_clock); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_EQ(RCL_RET_ERROR, rcl_enable_ros_time_override(&ros_clock)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_disable_ros_time_override(&ros_clock)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_is_enabled_ros_time_override(&ros_clock, &result)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_ERROR, rcl_set_ros_time_override(&ros_clock, set_point)); + rcl_reset_error(); +} diff --git a/test/rcl/test_timer.cpp b/test/rcl/test_timer.cpp new file mode 100644 index 0000000..66ed38c --- /dev/null +++ b/test/rcl/test_timer.cpp @@ -0,0 +1,906 @@ +// 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 + +#include "rcl/timer.h" + +#include "rcl/rcl.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" + +#include "./allocator_testing_utils.h" + +class TestTimerFixture : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "test_timer_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +static uint8_t times_called = 0; +static void callback_function(rcl_timer_t * timer, int64_t last_call) +{ + (void) timer; + (void) last_call; + times_called++; +} +static void callback_function_changed(rcl_timer_t * timer, int64_t last_call) +{ + (void) timer; + (void) last_call; + times_called--; +} + +class TestPreInitTimer : public TestTimerFixture +{ +public: + rcl_clock_t clock; + rcl_allocator_t allocator; + rcl_timer_t timer; + rcl_timer_callback_t timer_callback_test = &callback_function; + rcl_timer_callback_t timer_callback_changed = &callback_function_changed; + + void SetUp() override + { + TestTimerFixture::SetUp(); + rcl_ret_t ret; + allocator = rcl_get_default_allocator(); + timer = rcl_get_zero_initialized_timer(); + ASSERT_EQ( + RCL_RET_OK, + rcl_clock_init(RCL_ROS_TIME, &clock, &allocator)) << rcl_get_error_string().str; + + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, RCL_S_TO_NS(1), timer_callback_test, + rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() override + { + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string().str; + TestTimerFixture::TearDown(); + } +}; + +TEST_F(TestTimerFixture, test_timer_init_with_invalid_arguments) { + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + + ret = rcl_timer_init( + nullptr, &clock, this->context_ptr, RCL_MS_TO_NS(50), nullptr, allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_timer_init( + &timer, nullptr, this->context_ptr, RCL_MS_TO_NS(50), nullptr, allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_timer_init( + &timer, &clock, nullptr, RCL_MS_TO_NS(50), nullptr, allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, -1, nullptr, allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + + rcl_allocator_t invalid_allocator = rcutils_get_zero_initialized_allocator(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, RCL_MS_TO_NS(50), nullptr, invalid_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); +} + +TEST_F(TestTimerFixture, test_timer_with_invalid_clock) { + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_clock_init(RCL_CLOCK_UNINITIALIZED, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, 0, nullptr, allocator); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + + ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, 0, nullptr, allocator); + ASSERT_EQ(RCL_RET_OK, ret); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_clock_t * timer_clock; + ret = rcl_timer_clock(&timer, &timer_clock); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + timer_clock->get_now = nullptr; + + // Trigger clock jump callbacks + ret = rcl_enable_ros_time_override(timer_clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_timer_call(&timer); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + + int64_t time_until_next_call; + ret = rcl_timer_get_time_until_next_call(&timer, &time_until_next_call); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + + bool ready; + ret = rcl_timer_is_ready(&timer, &ready); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + + rcl_time_point_value_t time_since_last_call; + ret = rcl_timer_get_time_since_last_call(&timer, &time_since_last_call); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); + + ret = rcl_timer_reset(&timer); + EXPECT_EQ(RCL_RET_ERROR, ret); + rcl_reset_error(); +} + +TEST_F(TestTimerFixture, test_two_timers) { + rcl_ret_t ret; + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + rcl_timer_t timer2 = rcl_get_zero_initialized_timer(); + + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, RCL_MS_TO_NS(50), nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_timer_init( + &timer2, &clock, this->context_ptr, RCL_MS_TO_NS(1000), nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init(&wait_set, 0, 0, 2, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_timer(&wait_set, &timer2, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_timer_fini(&timer2); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + // The loop is needed because the rcl_wait_set might suffer spurious + // awakes when timers are involved. + // The loop can be removed if spurious awakes are fixed in the future. + // This issue particularly happens on Windows. + uint8_t nonnull_timers = 0; + auto start = std::chrono::system_clock::now(); + do { + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(100)); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + for (uint8_t i = 0; i < wait_set.size_of_timers; i++) { + if (wait_set.timers[i] != NULL) { + nonnull_timers++; + } + } + } while ( + nonnull_timers == 0u || + std::chrono::duration_cast( + std::chrono::system_clock::now() - start).count() > 100u); + bool is_ready = false; + ret = rcl_timer_is_ready(&timer, &is_ready); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(is_ready); + ret = rcl_timer_is_ready(&timer2, &is_ready); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_FALSE(is_ready); + ASSERT_EQ(1, nonnull_timers); + + ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +TEST_F(TestTimerFixture, test_two_timers_ready_before_timeout) { + rcl_ret_t ret; + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + rcl_timer_t timer2 = rcl_get_zero_initialized_timer(); + + // Keep the first timer period low enough so that rcl_wait() doesn't timeout too early. + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_timer_init( + &timer2, &clock, this->context_ptr, RCL_MS_TO_NS(1000), nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init(&wait_set, 0, 0, 2, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_timer(&wait_set, &timer2, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_timer_fini(&timer2); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + // The loop is needed because the rcl_wait_set might suffer spurious + // awakes when timers are involved. + // The loop can be removed if spurious awakes are fixed in the future. + // This issue particularly happens on Windows. + uint8_t nonnull_timers = 0u; + auto start = std::chrono::system_clock::now(); + do { + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(100)); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + for (uint8_t i = 0; i < wait_set.size_of_timers; i++) { + if (wait_set.timers[i] != NULL) { + nonnull_timers++; + } + } + } while ( + nonnull_timers == 0u || + std::chrono::duration_cast( + std::chrono::system_clock::now() - start).count() > 100u); + bool is_ready = false; + ret = rcl_timer_is_ready(&timer, &is_ready); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(is_ready); + ret = rcl_timer_is_ready(&timer2, &is_ready); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_FALSE(is_ready); + ASSERT_EQ(1, nonnull_timers); + + ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +TEST_F(TestTimerFixture, test_timer_not_ready) { + rcl_ret_t ret; + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, RCL_MS_TO_NS(1000), nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(100)); + EXPECT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str; + uint8_t nonnull_timers = 0; + for (uint8_t i = 0; i < wait_set.size_of_timers; i++) { + if (wait_set.timers[i] != NULL) { + nonnull_timers++; + } + } + bool is_ready = false; + ret = rcl_timer_is_ready(&timer, &is_ready); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_FALSE(is_ready); + ASSERT_EQ(0, nonnull_timers); + + ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +TEST_F(TestTimerFixture, test_timer_overrun) { + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, RCL_MS_TO_NS(200), nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + // Force multiple timer timeouts. + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(500)); + EXPECT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + bool is_ready = false; + ret = rcl_timer_is_ready(&timer, &is_ready); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(is_ready); + + EXPECT_EQ(RCL_RET_OK, rcl_timer_call(&timer)) << rcl_get_error_string().str; + + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Ensure period is re-aligned. + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(10)); + EXPECT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + ret = rcl_timer_is_ready(&timer, &is_ready); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_FALSE(is_ready); +} + +TEST_F(TestTimerFixture, test_timer_with_zero_period) { + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + rcl_ret_t ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, 0, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + bool is_ready = false; + ret = rcl_timer_is_ready(&timer, &is_ready); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(is_ready) << rcl_get_error_string().str; + + int64_t time_until_next_call = 0; + ret = rcl_timer_get_time_until_next_call(&timer, &time_until_next_call); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_LE(time_until_next_call, 0); + + EXPECT_EQ(RCL_RET_OK, rcl_timer_call(&timer)) << rcl_get_error_string().str; +} + +TEST_F(TestTimerFixture, test_canceled_timer) { + rcl_ret_t ret; + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, 500, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_timer_cancel(&timer); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + int64_t time_until_next_call = 0; + ret = rcl_timer_get_time_until_next_call(&timer, &time_until_next_call); + EXPECT_EQ(RCL_RET_TIMER_CANCELED, ret) << rcl_get_error_string().str; + + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(1)); + EXPECT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str; + uint8_t nonnull_timers = 0; + for (uint8_t i = 0; i < wait_set.size_of_timers; i++) { + if (wait_set.timers[i] != NULL) { + nonnull_timers++; + } + } + bool is_ready = false; + ret = rcl_timer_is_ready(&timer, &is_ready); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_FALSE(is_ready); + ASSERT_EQ(0, nonnull_timers); + + ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +TEST_F(TestTimerFixture, test_rostime_time_until_next_call) { + rcl_ret_t ret; + const int64_t sec_5 = RCL_S_TO_NS(5); + int64_t time_until = 0; + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string().str; + }); + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, sec_5, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string().str; + }); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, 1)) << rcl_get_error_string().str; + ret = rcl_timer_get_time_until_next_call(&timer, &time_until); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(sec_5 - 1, time_until); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_5)) << rcl_get_error_string().str; + ret = rcl_timer_get_time_until_next_call(&timer, &time_until); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(0, time_until); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_5 + 1)) << + rcl_get_error_string().str; + ret = rcl_timer_get_time_until_next_call(&timer, &time_until); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(-1, time_until); +} + +TEST_F(TestTimerFixture, test_system_time_to_ros_time) { + rcl_ret_t ret; + const int64_t sec_5 = RCL_S_TO_NS(5); + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string().str; + }); + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, sec_5, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string().str; + }); + + int64_t time_until_pre = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until_pre)) << + rcl_get_error_string().str; + ASSERT_LT(0, time_until_pre); + ASSERT_GT(sec_5, time_until_pre); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, 1)) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string().str; + + int64_t time_until = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) << + rcl_get_error_string().str; + // Because of time credit the time until next call should be less than before + EXPECT_GT(time_until_pre, time_until); + EXPECT_LT(0, time_until); +} + +TEST_F(TestTimerFixture, test_ros_time_to_system_time) { + rcl_ret_t ret; + const int64_t sec_5 = RCL_S_TO_NS(5); + const int64_t sec_1 = RCL_S_TO_NS(1); + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string().str; + }); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, 1)) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, sec_5, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string().str; + }); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1)) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string().str; + + int64_t time_until_pre = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until_pre)) << + rcl_get_error_string().str; + ASSERT_EQ(sec_5 - (sec_1 - 1), time_until_pre); + + ASSERT_EQ(RCL_RET_OK, rcl_disable_ros_time_override(&clock)) << rcl_get_error_string().str; + + int64_t time_until = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) << + rcl_get_error_string().str; + // Because of time credit the time until next call should be less than before + EXPECT_GT(time_until_pre, time_until); + EXPECT_LT(0, time_until); +} + +TEST_F(TestTimerFixture, test_ros_time_backwards_jump) { + rcl_ret_t ret; + const int64_t sec_5 = RCL_S_TO_NS(5); + const int64_t sec_3 = RCL_S_TO_NS(3); + const int64_t sec_2 = RCL_S_TO_NS(2); + const int64_t sec_1 = RCL_S_TO_NS(1); + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_ROS_TIME, &clock, &allocator); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string().str; + }); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_2)) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, sec_5, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string().str; + }); + + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_3)) << rcl_get_error_string().str; + { + // Moved forward a little bit, timer should be closer to being ready + int64_t time_until = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) << + rcl_get_error_string().str; + EXPECT_EQ(sec_5 - (sec_3 - sec_2), time_until); + } + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1)) << rcl_get_error_string().str; + { + // Jumped back before timer was created, so last_call_time should be 1 period + int64_t time_until = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &time_until)) << + rcl_get_error_string().str; + EXPECT_EQ(sec_5, time_until); + } +} + +TEST_F(TestTimerFixture, test_ros_time_wakes_wait) { + const int64_t sec_5 = RCL_S_TO_NS(5); + const int64_t sec_1 = RCL_S_TO_NS(1); + const int64_t sec_1_5 = RCL_S_TO_NS(3) / 2; + + rcl_ret_t ret; + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ASSERT_EQ(RCL_RET_OK, rcl_clock_init(RCL_ROS_TIME, &clock, &allocator)) << + rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_clock_fini(&clock)) << rcl_get_error_string().str; + }); + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1)) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&clock)) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, sec_1, nullptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(&timer)) << rcl_get_error_string().str; + }); + + bool timer_was_ready = false; + + std::thread wait_thr([&](void) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init( + &wait_set, + 0, 0, 1, 0, 0, 0, + context_ptr, + rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ASSERT_EQ(RCL_RET_OK, rcl_wait_set_add_timer(&wait_set, &timer, NULL)) << + rcl_get_error_string().str; + // *INDENT-OFF* (Uncrustify wants strange un-indentation here) + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << + rcl_get_error_string().str; + }); + // *INDENT-ON* + + ret = rcl_wait(&wait_set, sec_5); + // set some flag indicating wait was exited + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + if (wait_set.timers[0] != NULL) { + timer_was_ready = true; + } + }); + + // Timer not exceeded, should not wake + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_1_5)) << + rcl_get_error_string().str; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + EXPECT_FALSE(timer_was_ready); + + // Timer exceeded, should wake + ASSERT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&clock, sec_5)) << rcl_get_error_string().str; + auto start = std::chrono::steady_clock::now(); + wait_thr.join(); + auto finish = std::chrono::steady_clock::now(); + EXPECT_TRUE(timer_was_ready); + EXPECT_LT(finish - start, std::chrono::milliseconds(100)); +} + +TEST_F(TestPreInitTimer, test_timer_get_allocator) { + const rcl_allocator_t * allocator_returned = rcl_timer_get_allocator(&timer); + EXPECT_TRUE(rcutils_allocator_is_valid(allocator_returned)); + + EXPECT_EQ(NULL, rcl_timer_get_allocator(nullptr)); + rcl_reset_error(); +} + +TEST_F(TestPreInitTimer, test_timer_clock) { + rcl_clock_t * clock_impl = nullptr; + EXPECT_EQ(RCL_RET_OK, rcl_timer_clock(&timer, &clock_impl)) << rcl_get_error_string().str; + EXPECT_EQ(clock_impl, &clock); +} + +TEST_F(TestPreInitTimer, test_timer_call) { + int64_t next_call_start = 0; + int64_t next_call_end = 0; + int64_t old_period = 0; + times_called = 0; + + EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_start)); + ASSERT_EQ(RCL_RET_OK, rcl_timer_call(&timer)) << rcl_get_error_string().str; + EXPECT_EQ(times_called, 1); + + ASSERT_EQ(RCL_RET_OK, rcl_timer_call(&timer)) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, rcl_timer_call(&timer)) << rcl_get_error_string().str; + EXPECT_EQ(times_called, 3); + EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_end)); + EXPECT_GT(next_call_end, next_call_start); + + next_call_start = next_call_end; + ASSERT_EQ(RCL_RET_OK, rcl_timer_exchange_period(&timer, 0, &old_period)); + EXPECT_EQ(RCL_S_TO_NS(1), old_period); + ASSERT_EQ(RCL_RET_OK, rcl_timer_call(&timer)) << rcl_get_error_string().str; + EXPECT_EQ(times_called, 4); + EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_end)); + EXPECT_GT(next_call_start, next_call_end); + + EXPECT_EQ(RCL_RET_OK, rcl_enable_ros_time_override(&this->clock)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_set_ros_time_override(&this->clock, -1)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_ERROR, rcl_timer_call(&timer)); + rcl_reset_error(); + EXPECT_EQ(times_called, 4); + + EXPECT_EQ(RCL_RET_OK, rcl_timer_cancel(&timer)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_TIMER_CANCELED, rcl_timer_call(&timer)); + rcl_reset_error(); + EXPECT_EQ(times_called, 4); +} + +TEST_F(TestPreInitTimer, test_get_callback) { + ASSERT_EQ(timer_callback_test, rcl_timer_get_callback(&timer)) << rcl_get_error_string().str; +} + +TEST_F(TestPreInitTimer, test_timer_reset) { + int64_t next_call_start = 0; + int64_t next_call_end = 0; + times_called = 0; + + ASSERT_EQ(RCL_RET_OK, rcl_timer_call(&timer)) << rcl_get_error_string().str; + ASSERT_EQ(RCL_RET_OK, rcl_timer_call(&timer)) << rcl_get_error_string().str; + EXPECT_EQ(times_called, 2); + EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_start)); + + ASSERT_EQ(RCL_RET_OK, rcl_timer_reset(&timer)); + EXPECT_EQ(RCL_RET_OK, rcl_timer_get_time_until_next_call(&timer, &next_call_end)); + EXPECT_GT(next_call_start, next_call_end); + + ASSERT_EQ(RCL_RET_OK, rcl_timer_cancel(&timer)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_TIMER_CANCELED, rcl_timer_call(&timer)); + EXPECT_EQ(times_called, 2); + ASSERT_EQ(RCL_RET_OK, rcl_timer_reset(&timer)); + EXPECT_EQ(RCL_RET_OK, rcl_timer_call(&timer)) << rcl_get_error_string().str; + EXPECT_EQ(times_called, 3); +} + +TEST_F(TestPreInitTimer, test_timer_exchange_callback) { + times_called = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_call(&timer)) << rcl_get_error_string().str; + EXPECT_EQ(times_called, 1); + ASSERT_EQ( + timer_callback_test, rcl_timer_exchange_callback( + &timer, timer_callback_changed)) << rcl_get_error_string().str; + + ASSERT_EQ(RCL_RET_OK, rcl_timer_call(&timer)) << rcl_get_error_string().str; + EXPECT_EQ(times_called, 0); +} + +TEST_F(TestPreInitTimer, test_invalid_get_guard) { + ASSERT_EQ(NULL, rcl_timer_get_guard_condition(nullptr)); +} + +TEST_F(TestPreInitTimer, test_invalid_init_fini) { + rcl_allocator_t bad_allocator = get_failing_allocator(); + rcl_timer_t timer_fail = rcl_get_zero_initialized_timer(); + + EXPECT_EQ( + RCL_RET_ALREADY_INIT, rcl_timer_init( + &timer, &clock, this->context_ptr, 500, nullptr, + rcl_get_default_allocator())) << rcl_get_error_string().str; + rcl_reset_error(); + + ASSERT_EQ( + RCL_RET_BAD_ALLOC, rcl_timer_init( + &timer_fail, &clock, this->context_ptr, RCL_S_TO_NS(1), timer_callback_test, + bad_allocator)) << rcl_get_error_string().str; + rcl_reset_error(); + + EXPECT_EQ(RCL_RET_OK, rcl_timer_fini(nullptr)) << rcl_get_error_string().str; +} + +TEST_F(TestPreInitTimer, test_timer_get_period) { + int64_t period = 0; + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_period(&timer, &period)); + EXPECT_EQ(RCL_S_TO_NS(1), period); + + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_timer_get_period(nullptr, &period)); + rcl_reset_error(); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, rcl_timer_get_period(&timer, nullptr)); + rcl_reset_error(); +} + +TEST_F(TestPreInitTimer, test_time_since_last_call) { + rcl_time_point_value_t time_sice_next_call_start = 0u; + rcl_time_point_value_t time_sice_next_call_end = 0u; + + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_since_last_call(&timer, &time_sice_next_call_start)); + // Cope with coarse system time resolution. + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + ASSERT_EQ(RCL_RET_OK, rcl_timer_get_time_since_last_call(&timer, &time_sice_next_call_end)); + EXPECT_GT(time_sice_next_call_end, time_sice_next_call_start); +} diff --git a/test/rcl/test_two_executables.py.in b/test/rcl/test_two_executables.py.in new file mode 100644 index 0000000..8e56d56 --- /dev/null +++ b/test/rcl/test_two_executables.py.in @@ -0,0 +1,54 @@ +# generated from rcl/test/test_two_executables.py.in + +import os + +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch.actions import OpaqueFunction + +import launch_testing +import launch_testing.actions +import launch_testing.asserts + +import unittest + + +def generate_test_description(): + launch_description = LaunchDescription() + + launch_description.add_action( + ExecuteProcess( + cmd=['@TEST_EXECUTABLE1@'], + name='@TEST_EXECUTABLE1_NAME@', + ) + ) + + executable_under_test = ExecuteProcess( + cmd=['@TEST_EXECUTABLE2@', '@TEST_EXECUTABLE1_NAME@'], + name='@TEST_EXECUTABLE2_NAME@', + ) + launch_description.add_action(executable_under_test) + + launch_description.add_action( + launch_testing.actions.ReadyToTest() + ) + return launch_description, locals() + + +class TestTwoExecutables(unittest.TestCase): + + def @TEST_NAME@(self, executable_under_test, proc_info): + """Test that the executable under test terminates after a finite amount of time.""" + proc_info.assertWaitForShutdown(process=executable_under_test, timeout=10) + + +@launch_testing.post_shutdown_test() +class TestTwoExecutablesAfterShutdown(unittest.TestCase): + + def @TEST_NAME@(self, executable_under_test, proc_info): + """Test that the executable under test finished cleanly.""" + launch_testing.asserts.assertExitCodes( + proc_info, + [launch_testing.asserts.EXIT_OK], + executable_under_test + ) diff --git a/test/rcl/test_validate_enclave_name.cpp b/test/rcl/test_validate_enclave_name.cpp new file mode 100644 index 0000000..0cc4b1c --- /dev/null +++ b/test/rcl/test_validate_enclave_name.cpp @@ -0,0 +1,151 @@ +// 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 "rcutils/snprintf.h" + +#include "rcl/rcl.h" +#include "rcl/validate_enclave_name.h" + +#include "rcl/error_handling.h" + +#include "rmw/validate_namespace.h" + +#include "../mocking_utils/patch.hpp" + +TEST(TestValidateEnclaveName, test_validate) { + int validation_result; + size_t invalid_index; + + EXPECT_EQ( + RMW_RET_INVALID_ARGUMENT, + rcl_validate_enclave_name(nullptr, &validation_result, &invalid_index)); + + EXPECT_EQ( + RMW_RET_INVALID_ARGUMENT, + rcl_validate_enclave_name_with_size(nullptr, 20, &validation_result, &invalid_index)); + + EXPECT_EQ( + RMW_RET_INVALID_ARGUMENT, + rcl_validate_enclave_name_with_size("/foo", 20, nullptr, &invalid_index)); + + EXPECT_EQ( + RCL_RET_OK, + rcl_validate_enclave_name("/foo", &validation_result, &invalid_index)); + EXPECT_EQ(RCL_ENCLAVE_NAME_VALID, validation_result); + + EXPECT_EQ( + RCL_RET_OK, + rcl_validate_enclave_name("/foo/bar", &validation_result, &invalid_index)); + EXPECT_EQ(RCL_ENCLAVE_NAME_VALID, validation_result); + + { + auto mock = mocking_utils::patch( + "lib:rcl", rmw_validate_namespace_with_size, + [&](auto, auto, int * result, size_t * index) { + if (index) { + *index = 0u; + } + *result = RMW_NAMESPACE_INVALID_TOO_LONG; + return RMW_RET_OK; + }); + + // When applying RMW namespace validation rules, an enclave name may be too + // long for an RMW namespace but not necessarily for an enclave name. + EXPECT_EQ( + RCL_RET_OK, + rcl_validate_enclave_name("/foo/baz", &validation_result, &invalid_index)); + EXPECT_EQ(RCL_ENCLAVE_NAME_VALID, validation_result); + } +} + +TEST(TestValidateEnclaveName, test_validate_on_internal_error) { + int validation_result; + size_t invalid_index; + + { + auto mock = mocking_utils::patch_to_fail( + "lib:rcl", rmw_validate_namespace_with_size, "internal error", RMW_RET_ERROR); + + EXPECT_EQ( + RCL_RET_ERROR, + rcl_validate_enclave_name("/foo", &validation_result, &invalid_index)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } + + { + auto mock = mocking_utils::patch( + "lib:rcl", rmw_validate_namespace_with_size, + [&](auto, auto, int * result, size_t * index) { + if (index) { + *index = 0u; + } + *result = -1; + return RMW_RET_OK; + }); + + EXPECT_EQ( + RCL_RET_ERROR, + rcl_validate_enclave_name("/foo", &validation_result, &invalid_index)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } +} + +TEST(TestValidateEnclaveName, test_validation_string) { + struct enclave_case + { + std::string enclave; + int expected_validation_result; + size_t expected_invalid_index; + }; + std::vector enclave_cases_that_should_fail = { + // TODO(blast_545): Look for naming conventions doc for enclave_names + {"", RCL_ENCLAVE_NAME_INVALID_IS_EMPTY_STRING, 0}, + {"~/foo", RCL_ENCLAVE_NAME_INVALID_NOT_ABSOLUTE, 0}, + {"/foo/", RCL_ENCLAVE_NAME_INVALID_ENDS_WITH_FORWARD_SLASH, 4}, + {"/foo/$", RCL_ENCLAVE_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS, 5}, + {"/bar#", RCL_ENCLAVE_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS, 4}, + {"/foo//bar", RCL_ENCLAVE_NAME_INVALID_CONTAINS_REPEATED_FORWARD_SLASH, 5}, + {"/1bar", RCL_ENCLAVE_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER, 1}, + {"/" + std::string(RCL_ENCLAVE_NAME_MAX_LENGTH, 'o'), + RCL_ENCLAVE_NAME_INVALID_TOO_LONG, + RCL_ENCLAVE_NAME_MAX_LENGTH - 1} + }; + for (const auto & case_tuple : enclave_cases_that_should_fail) { + std::string enclave = case_tuple.enclave; + int expected_validation_result = case_tuple.expected_validation_result; + size_t expected_invalid_index = case_tuple.expected_invalid_index; + int validation_result; + size_t invalid_index = 0; + rcl_ret_t ret = rcl_validate_enclave_name(enclave.c_str(), &validation_result, &invalid_index); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(expected_validation_result, validation_result) << + "'" << enclave << "' should have failed with '" << + expected_validation_result << "' but got '" << validation_result << "'.\n" << + " " << std::string(invalid_index, ' ') << "^"; + EXPECT_EQ(expected_invalid_index, invalid_index) << + "Enclave '" << enclave << "' failed with '" << validation_result << "'."; + EXPECT_NE(nullptr, rcl_enclave_name_validation_result_string(validation_result)) << enclave; + } + EXPECT_STREQ( + "unknown result code for rcl context name validation", + rcl_enclave_name_validation_result_string(-1)); // invalid result + EXPECT_EQ(nullptr, rcl_enclave_name_validation_result_string(RCL_ENCLAVE_NAME_VALID)); +} diff --git a/test/rcl/test_validate_topic_name.cpp b/test/rcl/test_validate_topic_name.cpp new file mode 100644 index 0000000..eb4a2ce --- /dev/null +++ b/test/rcl/test_validate_topic_name.cpp @@ -0,0 +1,179 @@ +// 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 +#include + +#include "rcl/validate_topic_name.h" + +#include "rcl/error_handling.h" + +TEST(test_validate_topic_name, normal) { + rcl_ret_t ret; + + // passing without invalid_index + { + int validation_result; + ret = rcl_validate_topic_name("topic", &validation_result, nullptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_TOPIC_NAME_VALID, validation_result); + EXPECT_EQ(nullptr, rcl_topic_name_validation_result_string(validation_result)); + } + + // passing with invalid_index + { + int validation_result; + size_t invalid_index = 42; + ret = rcl_validate_topic_name("topic", &validation_result, &invalid_index); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_TOPIC_NAME_VALID, validation_result); + EXPECT_EQ(42u, invalid_index); // ensure invalid_index is not assigned on success + EXPECT_EQ(nullptr, rcl_topic_name_validation_result_string(validation_result)); + } + + // failing with invalid_index + { + int validation_result; + size_t invalid_index = 42; + ret = rcl_validate_topic_name("", &validation_result, &invalid_index); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_TOPIC_NAME_INVALID_IS_EMPTY_STRING, validation_result); + EXPECT_EQ(0u, invalid_index); + EXPECT_NE(nullptr, rcl_topic_name_validation_result_string(validation_result)); + } +} + +TEST(test_validate_topic_name, invalid_arguments) { + rcl_ret_t ret; + + // pass null for topic string + { + int validation_result; + ret = rcl_validate_topic_name(nullptr, &validation_result, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + } + + // pass null for validation_result + { + ret = rcl_validate_topic_name("topic", nullptr, nullptr); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + rcl_reset_error(); + } +} + +TEST(test_validate_topic_name, various_valid_topics) { + std::vector topics_that_should_pass = { + // examples from the design doc: + // http://design.ros2.org/articles/topic_and_service_names.html#ros-2-name-examples + "foo", + "abc123", + "_foo", + "Foo", + "BAR", + "~", + "foo/bar", + "~/foo", + "{foo}_bar", + "foo/{ping}/bar", + "foo/_bar", + "foo_/bar", + "foo_", + // these two are skipped because their prefixes should be removed before this is called + // "rosservice:///foo", + // "rostopic://foo/bar", + "/foo", + "/bar/baz", + // same reason as above, URL should have been removed already + // "rostopic:///ping", + "/_private/thing", + "/public_namespace/_private/thing", + // these are further corner cases identified: + "/foo", + "{foo1}", + "{foo_bar}", + "{_bar}", + }; + for (const auto & topic : topics_that_should_pass) { + int validation_result; + size_t invalid_index = 42; + rcl_ret_t ret = rcl_validate_topic_name(topic.c_str(), &validation_result, &invalid_index); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(RCL_TOPIC_NAME_VALID, validation_result) << + "'" << topic << "' should have passed: " << + rcl_topic_name_validation_result_string(validation_result) << "\n" << + " " << std::string(invalid_index, ' ') << "^"; + EXPECT_EQ(42u, invalid_index); + EXPECT_STREQ(nullptr, rcl_topic_name_validation_result_string(validation_result)); + } + + int not_valid_validation_result = 5600; + EXPECT_STREQ( + "unknown result code for rcl topic name validation", + rcl_topic_name_validation_result_string(not_valid_validation_result)); +} + +TEST(test_validate_topic_name, various_invalid_topics) { + struct topic_case + { + std::string topic; + int expected_validation_result; + size_t expected_invalid_index; + }; + std::vector topic_cases_that_should_fail = { + // examples from the design doc: + // http://design.ros2.org/articles/topic_and_service_names.html#ros-2-name-examples + {"123abc", RCL_TOPIC_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER, 0}, + {"123", RCL_TOPIC_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER, 0}, + {" ", RCL_TOPIC_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS, 0}, + {"foo bar", RCL_TOPIC_NAME_INVALID_CONTAINS_UNALLOWED_CHARACTERS, 3}, + // this one is skipped because it tested later, after expansion + // "foo//bar", + {"/~", RCL_TOPIC_NAME_INVALID_MISPLACED_TILDE, 1}, + {"~foo", RCL_TOPIC_NAME_INVALID_TILDE_NOT_FOLLOWED_BY_FORWARD_SLASH, 1}, + {"foo~", RCL_TOPIC_NAME_INVALID_MISPLACED_TILDE, 3}, + {"foo~/bar", RCL_TOPIC_NAME_INVALID_MISPLACED_TILDE, 3}, + {"foo/~bar", RCL_TOPIC_NAME_INVALID_MISPLACED_TILDE, 4}, + {"foo/~/bar", RCL_TOPIC_NAME_INVALID_MISPLACED_TILDE, 4}, + {"foo/", RCL_TOPIC_NAME_INVALID_ENDS_WITH_FORWARD_SLASH, 3}, + // these are further corner cases identified: + {"", RCL_TOPIC_NAME_INVALID_IS_EMPTY_STRING, 0}, + {"foo/123bar", RCL_TOPIC_NAME_INVALID_NAME_TOKEN_STARTS_WITH_NUMBER, 4}, + {"foo/bar}/baz", RCL_TOPIC_NAME_INVALID_UNMATCHED_CURLY_BRACE, 7}, + {"foo/{bar", RCL_TOPIC_NAME_INVALID_UNMATCHED_CURLY_BRACE, 4}, + {"{$}", RCL_TOPIC_NAME_INVALID_SUBSTITUTION_CONTAINS_UNALLOWED_CHARACTERS, 1}, + {"{{bar}_baz}", RCL_TOPIC_NAME_INVALID_SUBSTITUTION_CONTAINS_UNALLOWED_CHARACTERS, 1}, + {"foo/{bar/baz}", RCL_TOPIC_NAME_INVALID_SUBSTITUTION_CONTAINS_UNALLOWED_CHARACTERS, 8}, + {"{1foo}", RCL_TOPIC_NAME_INVALID_SUBSTITUTION_STARTS_WITH_NUMBER, 1}, + }; + for (const auto & case_tuple : topic_cases_that_should_fail) { + std::string topic = case_tuple.topic; + int expected_validation_result = case_tuple.expected_validation_result; + size_t expected_invalid_index = case_tuple.expected_invalid_index; + int validation_result; + size_t invalid_index = 0; + rcl_ret_t ret = rcl_validate_topic_name(topic.c_str(), &validation_result, &invalid_index); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(expected_validation_result, validation_result) << + "'" << topic << "' should have failed with '" << + expected_validation_result << "' but got '" << validation_result << "'.\n" << + " " << std::string(invalid_index, ' ') << "^"; + EXPECT_EQ(expected_invalid_index, invalid_index) << + "Topic '" << topic << "' failed with '" << validation_result << "'."; + EXPECT_NE(nullptr, rcl_topic_name_validation_result_string(validation_result)) << topic; + } +} diff --git a/test/rcl/test_wait.cpp b/test/rcl/test_wait.cpp new file mode 100644 index 0000000..11fe07c --- /dev/null +++ b/test/rcl/test_wait.cpp @@ -0,0 +1,778 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include // for std::max +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rcl/rcl.h" +#include "rcl/wait.h" + +#include "rcutils/logging_macros.h" + +#include "./allocator_testing_utils.h" +#include "../mocking_utils/patch.hpp" + +#ifdef RMW_IMPLEMENTATION +# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX +# define CLASSNAME(NAME, SUFFIX) CLASSNAME_(NAME, SUFFIX) +#else +# define CLASSNAME(NAME, SUFFIX) NAME +#endif + +#ifndef _WIN32 +#define TOLERANCE RCL_MS_TO_NS(6) +#else +#define TOLERANCE RCL_MS_TO_NS(25) +#endif + +class CLASSNAME (WaitSetTestFixture, RMW_IMPLEMENTATION) : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + void SetUp() + { + rcl_ret_t ret; + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + EXPECT_EQ(RCL_RET_OK, rcl_shutdown(this->context_ptr)) << rcl_get_error_string().str; + EXPECT_EQ(RCL_RET_OK, rcl_context_fini(this->context_ptr)) << rcl_get_error_string().str; + delete this->context_ptr; + } +}; + +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), wait_set_is_valid) { + // null pointers are invalid + EXPECT_FALSE(rcl_wait_set_is_valid(nullptr)); + + // uninitialized wait set is invalid + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + EXPECT_FALSE(rcl_wait_set_is_valid(&wait_set)); + + // initialized wait set is valid + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_TRUE(rcl_wait_set_is_valid(&wait_set)); + + // finalized wait set is invalid + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_FALSE(rcl_wait_set_is_valid(&wait_set)); +} + +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_failed_resize) { + // Initialize a wait set with a subscription and then resize it to zero. + rcl_allocator_t allocator = get_failing_allocator(); + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + set_failing_allocator_is_failing(allocator, false); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, allocator); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + set_failing_allocator_is_failing(allocator, true); + ret = rcl_wait_set_resize(&wait_set, 0, 1, 0, 0, 0, 0); + EXPECT_EQ(RCL_RET_BAD_ALLOC, ret); + rcl_reset_error(); + + set_failing_allocator_is_failing(allocator, false); + ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_resize_to_zero) { + // Initialize a wait set with a subscription and then resize it to zero. + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + ret = rcl_wait_set_resize(&wait_set, 0u, 0u, 0u, 0u, 0u, 0u); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + EXPECT_EQ(wait_set.size_of_subscriptions, 0ull); + EXPECT_EQ(wait_set.size_of_guard_conditions, 0ull); + EXPECT_EQ(wait_set.size_of_clients, 0ull); + EXPECT_EQ(wait_set.size_of_services, 0ull); + EXPECT_EQ(wait_set.size_of_timers, 0ull); + + ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +// Test rcl_wait with a positive finite timeout value (1ms) +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), finite_timeout) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + int64_t timeout = RCL_MS_TO_NS(10); // nanoseconds + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + ret = rcl_wait(&wait_set, timeout); + std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now(); + ASSERT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str; + // Check time + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + EXPECT_LE(diff, timeout + TOLERANCE); + + ret = rcl_wait_set_fini(&wait_set); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +// Check that a timer overrides a negative timeout value (blocking forever) +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), negative_timeout) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 1, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + // Add a dummy guard condition to avoid an error + rcl_guard_condition_t guard_cond = rcl_get_zero_initialized_guard_condition(); + ret = rcl_guard_condition_init( + &guard_cond, this->context_ptr, rcl_guard_condition_get_default_options()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_guard_condition_fini(&guard_cond); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + int64_t timeout = -1; + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + ret = rcl_wait(&wait_set, timeout); + std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now(); + // We expect a timeout here (timer value reached) + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // Check time + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + EXPECT_LE(diff, RCL_MS_TO_NS(10) + TOLERANCE); +} + +// Test rcl_wait with a timeout value of 0 (non-blocking) +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), zero_timeout) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 1, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + // Add a dummy guard condition to avoid an error + rcl_guard_condition_t guard_cond = rcl_get_zero_initialized_guard_condition(); + ret = rcl_guard_condition_init( + &guard_cond, this->context_ptr, rcl_guard_condition_get_default_options()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_guard_condition_fini(&guard_cond); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Time spent during wait should be negligible. + int64_t timeout = 0; + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + ret = rcl_wait(&wait_set, timeout); + std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now(); + // We expect a timeout here (timer value reached) + ASSERT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str; + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + EXPECT_LE(diff, TOLERANCE); +} + +// Test rcl_wait with a timeout value of 0 (non-blocking) and an already triggered guard condition +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), zero_timeout_triggered_guard_condition) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 1, 0, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_guard_condition_t guard_cond = rcl_get_zero_initialized_guard_condition(); + ret = rcl_guard_condition_init( + &guard_cond, this->context_ptr, rcl_guard_condition_get_default_options()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_guard_condition_fini(&guard_cond); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_trigger_guard_condition(&guard_cond); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + +// Time spent during wait should be negligible. + int64_t timeout = 0; + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + ret = rcl_wait(&wait_set, timeout); + std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now(); + // We don't expect a timeout here (since the guard condition had already been triggered) + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + EXPECT_LE(diff, TOLERANCE); +} + +// Test rcl_wait with a timeout value and an overrun timer +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), zero_timeout_overrun_timer) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_timer_t timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &timer, &clock, this->context_ptr, 0, nullptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_timer_fini(&timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Time spent during wait should be negligible, definitely less than the given timeout + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(100)); + std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now(); + // We don't expect a timeout here (since the guard condition had already been triggered) + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + EXPECT_LE(diff, RCL_MS_TO_NS(50)); +} + +// Check that a canceled timer doesn't wake up rcl_wait +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), canceled_timer) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 1, 1, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + // Add a dummy guard condition to avoid an error + rcl_guard_condition_t guard_cond = rcl_get_zero_initialized_guard_condition(); + ret = rcl_guard_condition_init( + &guard_cond, this->context_ptr, rcl_guard_condition_get_default_options()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_guard_condition_fini(&guard_cond); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_clock_t clock; + rcl_allocator_t allocator = rcl_get_default_allocator(); + ret = rcl_clock_init(RCL_STEADY_TIME, &clock, &allocator); + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_clock_fini(&clock); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + rcl_timer_t canceled_timer = rcl_get_zero_initialized_timer(); + ret = rcl_timer_init( + &canceled_timer, &clock, this->context_ptr, + RCL_MS_TO_NS(1), nullptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_timer_fini(&canceled_timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_timer_cancel(&canceled_timer); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_timer(&wait_set, &canceled_timer, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + int64_t timeout = RCL_MS_TO_NS(10); + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + ret = rcl_wait(&wait_set, timeout); + std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now(); + // We expect a timeout here (timer value reached) + ASSERT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str; + // Check time + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + EXPECT_LE(diff, RCL_MS_TO_NS(10) + TOLERANCE); +} + +// Test rcl_wait_set_t with excess capacity works. +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), excess_capacity) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 42, 42, 42, 42, 42, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + int64_t timeout = 1; + ret = rcl_wait(&wait_set, timeout); + ASSERT_EQ(RCL_RET_TIMEOUT, ret) << rcl_get_error_string().str; +} + +// Check rcl_wait can be called in many threads, each with unique wait sets and resources. +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), multi_wait_set_threaded) { + rcl_ret_t ret; + const size_t number_of_threads = 20; // concurrent waits + const size_t count_target = 10; // number of times each wait should wake up before being "done" + const size_t retry_limit = 100; // number of times to retry when a timeout occurs waiting + const uint64_t wait_period = RCL_MS_TO_NS(100); // timeout passed to rcl_wait each try + const std::chrono::milliseconds trigger_period(2); // period between each round of triggers + struct TestSet + { + std::atomic wake_count; + rcl_wait_set_t wait_set; + rcl_guard_condition_t guard_condition; + std::thread thread; + size_t thread_id; + }; + std::vector test_sets(number_of_threads); + // *INDENT-OFF* (prevent uncrustify from making unnecessary whitespace around [=]) + // Setup common function for waiting on the triggered guard conditions. + auto wait_func_factory = + [=](TestSet & test_set) { + return + [=, &test_set]() { + while (test_set.wake_count < count_target) { + bool change_detected = false; + size_t wake_try_count = 0; + while (wake_try_count < retry_limit) { + wake_try_count++; + rcl_ret_t ret; + ret = rcl_wait_set_clear(&test_set.wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_guard_condition( + &test_set.wait_set, &test_set.guard_condition, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait(&test_set.wait_set, wait_period); + if (ret != RCL_RET_TIMEOUT) { + ASSERT_EQ(ret, RCL_RET_OK); + change_detected = true; + // if not timeout, then the single guard condition should be set + if (!test_set.wait_set.guard_conditions[0]) { + test_set.wake_count.store(count_target + 1); // indicates an error + ASSERT_NE(nullptr, test_set.wait_set.guard_conditions[0]) << + "[thread " << test_set.thread_id << + "] expected guard condition to be marked ready after non-timeout wake up"; + } + // no need to wait again + break; + } else { + std::stringstream ss; + ss << "[thread " << test_set.thread_id << "] Timeout (try #" << wake_try_count << + ")"; + // TODO(mikaelarguedas) replace this with stream logging once they exist + RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "%s", ss.str().c_str()); + } + } + if (!change_detected) { + test_set.wake_count.store(count_target + 1); // indicates an error + ASSERT_TRUE(change_detected); + } + test_set.wake_count++; + } + }; + }; + // *INDENT-ON* + // Setup each test set. + for (auto & test_set : test_sets) { + rcl_ret_t ret; + // init the wake count + test_set.wake_count.store(0); + // setup the guard condition + test_set.guard_condition = rcl_get_zero_initialized_guard_condition(); + ret = rcl_guard_condition_init( + &test_set.guard_condition, this->context_ptr, rcl_guard_condition_get_default_options()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // guard_condition must live longer than this loop. Call fini from function-level exit-scope. + // setup the wait set + test_set.wait_set = rcl_get_zero_initialized_wait_set(); + ret = rcl_wait_set_init( + &test_set.wait_set, 0, 1, 0, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // wait_set must live longer than this loop. Call fini from function-level exit-scope. + ret = rcl_wait_set_add_guard_condition(&test_set.wait_set, &test_set.guard_condition, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + test_set.thread_id = 0; + } + // Setup safe tear-down. + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + for (auto & test_set : test_sets) { + rcl_ret_t ret = rcl_guard_condition_fini(&test_set.guard_condition); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_fini(&test_set.wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + }); + // Now kick off all the threads. + size_t thread_enumeration = 0; + for (auto & test_set : test_sets) { + thread_enumeration++; + test_set.thread_id = thread_enumeration; + test_set.thread = std::thread(wait_func_factory(test_set)); + } + // Loop, triggering every trigger_period until the threads are done. + // *INDENT-OFF* (prevent uncrustify from making unnecessary whitespace around [=]) + auto loop_test = + [=, &test_sets]() -> bool { + for (const auto & test_set : test_sets) { + if (test_set.wake_count.load() < count_target) { + return true; + } + } + return false; + }; + // *INDENT-ON* + size_t loop_count = 0; + while (loop_test()) { + loop_count++; + for (auto & test_set : test_sets) { + ret = rcl_trigger_guard_condition(&test_set.guard_condition); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + std::this_thread::sleep_for(trigger_period); + } + // Join the threads. + for (auto & test_set : test_sets) { + test_set.thread.join(); + } + // Ensure the individual wake counts match the target (otherwise there was a problem). + for (auto & test_set : test_sets) { + ASSERT_EQ(count_target, test_set.wake_count.load()); + } +} + +// Check the interaction of a guard condition and a negative timeout by +// triggering a guard condition in a separate thread +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), guard_condition) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 1, 0, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + rcl_guard_condition_t guard_cond = rcl_get_zero_initialized_guard_condition(); + ret = rcl_guard_condition_init( + &guard_cond, this->context_ptr, rcl_guard_condition_get_default_options()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_guard_condition_fini(&guard_cond); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond, NULL); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + std::promise p; + + int64_t timeout = -1; + + std::chrono::nanoseconds trigger_diff; + std::thread trigger_thread( + [&p, &guard_cond, &trigger_diff]() { + std::chrono::steady_clock::time_point before_trigger = std::chrono::steady_clock::now(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + rcl_ret_t ret = rcl_trigger_guard_condition(&guard_cond); + std::chrono::steady_clock::time_point after_trigger = std::chrono::steady_clock::now(); + trigger_diff = std::chrono::duration_cast( + after_trigger - before_trigger); + p.set_value(ret); + }); + auto f = p.get_future(); + + std::chrono::steady_clock::time_point before_sc = std::chrono::steady_clock::now(); + ret = rcl_wait(&wait_set, timeout); + std::chrono::steady_clock::time_point after_sc = std::chrono::steady_clock::now(); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + int64_t diff = std::chrono::duration_cast(after_sc - before_sc).count(); + trigger_thread.join(); + EXPECT_EQ(RCL_RET_OK, f.get()); + EXPECT_LE(std::abs(diff - trigger_diff.count()), TOLERANCE); +} + +// Check that index arguments are properly set when adding entities +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), add_with_index) { + const size_t kNumEntities = 3u; + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = rcl_wait_set_init( + &wait_set, 0, kNumEntities, 0, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + // Initialize to invalid value + size_t guard_condition_index = 42u; + + rcl_guard_condition_t guard_conditions[kNumEntities]; + for (size_t i = 0u; i < kNumEntities; ++i) { + guard_conditions[i] = rcl_get_zero_initialized_guard_condition(); + ret = rcl_guard_condition_init( + &guard_conditions[i], this->context_ptr, rcl_guard_condition_get_default_options()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + ret = rcl_guard_condition_fini(&guard_conditions[i]); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + ret = rcl_wait_set_add_guard_condition( + &wait_set, &guard_conditions[i], &guard_condition_index); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(guard_condition_index, i); + EXPECT_EQ(&guard_conditions[i], wait_set.guard_conditions[i]); + } +} + +// Extra invalid arguments not tested +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), wait_set_valid_arguments) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 0, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_WAIT_SET_EMPTY, rcl_wait(&wait_set, RCL_MS_TO_NS(1000))) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, rcl_wait(nullptr, RCL_MS_TO_NS(1000))) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_WAIT_SET_INVALID, + rcl_wait(&wait_set, RCL_MS_TO_NS(1000))) << rcl_get_error_string().str; + rcl_reset_error(); + + rcl_context_t not_init_context = rcl_get_zero_initialized_context(); + ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, ¬_init_context, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_NOT_INIT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + + // nullptr failures + ret = + rcl_wait_set_init(nullptr, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, nullptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + rcl_allocator_t zero_init_allocator = + static_cast(rcutils_get_zero_initialized_allocator()); + ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, zero_init_allocator); + EXPECT_EQ(RCL_RET_INVALID_ARGUMENT, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + + ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_ALREADY_INIT, ret) << rcl_get_error_string().str; + rcl_reset_error(); + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +// Test get allocator function +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), wait_set_get_allocator) { + rcl_allocator_t allocator_returned; + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_wait_set_get_allocator(nullptr, &allocator_returned)) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ( + RCL_RET_WAIT_SET_INVALID, + rcl_wait_set_get_allocator(&wait_set, &allocator_returned)) << rcl_get_error_string().str; + rcl_reset_error(); + + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ( + RCL_RET_INVALID_ARGUMENT, + rcl_wait_set_get_allocator(&wait_set, nullptr)) << rcl_get_error_string().str; + rcl_reset_error(); + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_get_allocator(&wait_set, &allocator_returned)); + EXPECT_TRUE(rcutils_allocator_is_valid(&allocator_returned)); + + ret = rcl_wait_set_fini(&wait_set); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; +} + +// Test wait set init failure cases using mocks +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), wait_set_failed_init) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + // Mock rmw implementation to fail init + auto mock = mocking_utils::patch_and_return( + "lib:rcl", rmw_create_wait_set, nullptr); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_WAIT_SET_INVALID, ret); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); +} + +// Test wait set fini failure cases using mocks +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), wait_set_failed_fini) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 0, context_ptr, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret); + { + // Mock rmw implementation to fail fini + auto mock = mocking_utils::inject_on_return( + "lib:rcl", rmw_destroy_wait_set, RMW_RET_ERROR); + EXPECT_EQ(RCL_RET_WAIT_SET_INVALID, rcl_wait_set_fini(&wait_set)); + EXPECT_TRUE(rcl_error_is_set()); + rcl_reset_error(); + } +} + +// Test proper error handling with a fault injection test +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), wait_set_test_maybe_init_fail) { + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = RCL_RET_OK; + rcl_allocator_t alloc = rcl_get_default_allocator(); + + RCUTILS_FAULT_INJECTION_TEST( + { + // Test init in the case where guard_conditions_size + timers_size = 0 + // (used for guard condition size) + ret = rcl_wait_set_init(&wait_set, 1, 0, 0, 1, 1, 0, context_ptr, alloc); + if (RCL_RET_OK == ret) { + ret = rcl_wait_set_fini(&wait_set); + if (ret != RCL_RET_OK) { + rcl_reset_error(); + } + } else { + EXPECT_TRUE(RCL_RET_WAIT_SET_INVALID == ret || RCL_RET_BAD_ALLOC == ret); + rcl_reset_error(); + } + }); + + RCUTILS_FAULT_INJECTION_TEST( + { + // Test init wait_set using at least one of each of the possible elements that receives + // (subs, guard conditions, timers, clients, services, events) + ret = rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, 1, context_ptr, alloc); + if (RCL_RET_OK == ret) { + ret = rcl_wait_set_fini(&wait_set); + if (ret != RCL_RET_OK) { + rcl_reset_error(); + } + EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)); + } else { + EXPECT_TRUE(RCL_RET_WAIT_SET_INVALID == ret || RCL_RET_BAD_ALLOC == ret); + rcl_reset_error(); + } + }); +} diff --git a/test/rcl/wait_for_entity_helpers.cpp b/test/rcl/wait_for_entity_helpers.cpp new file mode 100644 index 0000000..625e1ea --- /dev/null +++ b/test/rcl/wait_for_entity_helpers.cpp @@ -0,0 +1,238 @@ +// 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 "wait_for_entity_helpers.hpp" + +#include +#include +#include + +#include "rcutils/logging_macros.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rcl/graph.h" + +bool +wait_for_server_to_be_available( + rcl_node_t * node, + rcl_client_t * client, + size_t max_tries, + int64_t period_ms) +{ + size_t iteration = 0; + while (iteration < max_tries) { + ++iteration; + bool is_ready; + rcl_ret_t ret = rcl_service_server_is_available(node, client, &is_ready); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Error in rcl_service_server_is_available: %s", + rcl_get_error_string().str); + return false; + } + if (is_ready) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + return false; +} + +bool +wait_for_client_to_be_ready( + rcl_client_t * client, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms) +{ + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, 0, context, rcl_get_default_allocator()); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str); + return false; + } + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str); + throw std::runtime_error("error while waiting for client"); + } + }); + size_t iteration = 0; + while (iteration < max_tries) { + ++iteration; + if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str); + return false; + } + if (rcl_wait_set_add_client(&wait_set, client, NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait_set_add_client: %s", rcl_get_error_string().str); + return false; + } + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); + if (ret == RCL_RET_TIMEOUT) { + continue; + } + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str); + return false; + } + for (size_t i = 0; i < wait_set.size_of_clients; ++i) { + if (wait_set.clients[i] && wait_set.clients[i] == client) { + return true; + } + } + } + return false; +} + +bool +wait_for_service_to_be_ready( + rcl_service_t * service, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms) +{ + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, 0, context, rcl_get_default_allocator()); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str); + return false; + } + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str); + throw std::runtime_error("error waiting for service to be ready"); + } + }); + size_t iteration = 0; + while (iteration < max_tries) { + ++iteration; + if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str); + return false; + } + if (rcl_wait_set_add_service(&wait_set, service, NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait_set_add_service: %s", rcl_get_error_string().str); + return false; + } + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); + if (ret == RCL_RET_TIMEOUT) { + continue; + } + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str); + return false; + } + for (size_t i = 0; i < wait_set.size_of_services; ++i) { + if (wait_set.services[i] && wait_set.services[i] == service) { + return true; + } + } + } + return false; +} + +bool +wait_for_established_subscription( + const rcl_publisher_t * publisher, + size_t max_tries, + int64_t period_ms) +{ + size_t iteration = 0; + rcl_ret_t ret = RCL_RET_OK; + size_t subscription_count = 0; + while (iteration < max_tries) { + ++iteration; + ret = rcl_publisher_get_subscription_count(publisher, &subscription_count); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Error in rcl_publisher_get_subscription_count: %s", + rcl_get_error_string().str); + return false; + } + if (subscription_count > 0) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + return false; +} + +bool +wait_for_subscription_to_be_ready( + rcl_subscription_t * subscription, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms) +{ + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, context, rcl_get_default_allocator()); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str); + return false; + } + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str); + throw std::runtime_error("error waiting for service to be ready"); + } + }); + size_t iteration = 0; + while (iteration < max_tries) { + ++iteration; + if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str); + return false; + } + if (rcl_wait_set_add_subscription(&wait_set, subscription, NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in rcl_wait_set_add_subscription: %s", rcl_get_error_string().str); + return false; + } + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); + if (ret == RCL_RET_TIMEOUT) { + continue; + } + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str); + return false; + } + for (size_t i = 0; i < wait_set.size_of_subscriptions; ++i) { + if (wait_set.subscriptions[i] && wait_set.subscriptions[i] == subscription) { + return true; + } + } + } + return false; +} diff --git a/test/rcl/wait_for_entity_helpers.hpp b/test/rcl/wait_for_entity_helpers.hpp new file mode 100644 index 0000000..35baa64 --- /dev/null +++ b/test/rcl/wait_for_entity_helpers.hpp @@ -0,0 +1,66 @@ +// 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 RCL__WAIT_FOR_ENTITY_HELPERS_HPP_ +#define RCL__WAIT_FOR_ENTITY_HELPERS_HPP_ + +#include "rcl/client.h" +#include "rcl/service.h" +#include "rcl/rcl.h" + +/// Wait for a server to be available for `client`, by trying at most `max_tries` times +/// with a `period_ms` period. +bool +wait_for_server_to_be_available( + rcl_node_t * node, + rcl_client_t * client, + size_t max_tries, + int64_t period_ms); + +/// Wait for `client` to be ready, i.e. a response is available to be handled. +/// It's tried at most `max_tries` times with a period of `period_ms`. +bool +wait_for_client_to_be_ready( + rcl_client_t * client, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms); + +/// Wait for service to be ready, i.e. a request is available to be handled. +/// It's tried at most `max_tries` times with a period of `period_ms`. +bool +wait_for_service_to_be_ready( + rcl_service_t * service, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms); + +/// Wait for a publisher to get one or more established subscriptions +/// by trying at most `max_tries` times with a `period_ms` period. +bool +wait_for_established_subscription( + const rcl_publisher_t * publisher, + size_t max_tries, + int64_t period_ms); + +/// Wait a subscription to be ready, i.e. a message is ready to be handled, +/// by trying at least `max_tries` times with a `period_ms` period. +bool +wait_for_subscription_to_be_ready( + rcl_subscription_t * subscription, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms); + +#endif // RCL__WAIT_FOR_ENTITY_HELPERS_HPP_ diff --git a/test/resources/test_arguments/test_parameters.1.yaml b/test/resources/test_arguments/test_parameters.1.yaml new file mode 100644 index 0000000..8c8faed --- /dev/null +++ b/test/resources/test_arguments/test_parameters.1.yaml @@ -0,0 +1,5 @@ +some_node: + ros__parameters: + int_param: 1 + param_group: + string_param: foo diff --git a/test/resources/test_arguments/test_parameters.2.yaml b/test/resources/test_arguments/test_parameters.2.yaml new file mode 100644 index 0000000..e1e3c16 --- /dev/null +++ b/test/resources/test_arguments/test_parameters.2.yaml @@ -0,0 +1,8 @@ +some_node: + ros__parameters: + int_param: 3 +another_node: + ros__parameters: + double_param: 1.0 + param_group: + bool_array_param: [true, false, false] diff --git a/test/resources/test_profile/disable_intraprocess.xml b/test/resources/test_profile/disable_intraprocess.xml new file mode 100644 index 0000000..8c1a92f --- /dev/null +++ b/test/resources/test_profile/disable_intraprocess.xml @@ -0,0 +1,3 @@ + + OFF + diff --git a/test/resources/test_security_directory/enclaves/dummy_enclave/.gitkeep b/test/resources/test_security_directory/enclaves/dummy_enclave/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/test/resources/test_security_directory/enclaves/group1/dummy_enclave/.gitkeep b/test/resources/test_security_directory/enclaves/group1/dummy_enclave/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/test/test_namespace.cpp b/test/test_namespace.cpp new file mode 100644 index 0000000..3252e3e --- /dev/null +++ b/test/test_namespace.cpp @@ -0,0 +1,139 @@ +// 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 + +#include "rcl/client.h" + +#include "rcl/rcl.h" +#include "rcl/graph.h" + +#include "rosidl_runtime_c/string_functions.h" +#include "test_msgs/srv/basic_types.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" + +using namespace std::chrono_literals; + +class TestNamespaceFixture : public ::testing::Test +{ +public: + rcl_context_t * context_ptr; + rcl_node_t * node_ptr; + void SetUp() + { + rcl_ret_t ret; + { + rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); + ret = rcl_init_options_init(&init_options, rcl_get_default_allocator()); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + EXPECT_EQ(RCL_RET_OK, rcl_init_options_fini(&init_options)) << rcl_get_error_string().str; + }); + this->context_ptr = new rcl_context_t; + *this->context_ptr = rcl_get_zero_initialized_context(); + ret = rcl_init(0, nullptr, &init_options, this->context_ptr); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + this->node_ptr = new rcl_node_t; + *this->node_ptr = rcl_get_zero_initialized_node(); + const char * name = "test_namespace_node"; + rcl_node_options_t node_options = rcl_node_get_default_options(); + ret = rcl_node_init(this->node_ptr, name, "", this->context_ptr, &node_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } + + void TearDown() + { + rcl_ret_t ret = rcl_node_fini(this->node_ptr); + delete this->node_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_shutdown(this->context_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_context_fini(this->context_ptr); + delete this->context_ptr; + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + } +}; + +/* Basic nominal test of a client. + */ +TEST_F(TestNamespaceFixture, test_client_server) { + rcl_ret_t ret; + auto ts = ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes); + const char * service_name = "/my/namespace/test_namespace_client_server"; + const char * unmatched_client_name = "/your/namespace/test_namespace_client_server"; + const char * matched_client_name = "/my/namespace/test_namespace_client_server"; + auto timeout = 10; + + rcl_service_t service = rcl_get_zero_initialized_service(); + rcl_service_options_t service_options = rcl_service_get_default_options(); + ret = rcl_service_init(&service, this->node_ptr, ts, service_name, &service_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_service_fini(&service, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + rcl_client_t unmatched_client = rcl_get_zero_initialized_client(); + rcl_client_options_t unmatched_client_options = rcl_client_get_default_options(); + ret = rcl_client_init( + &unmatched_client, this->node_ptr, ts, unmatched_client_name, &unmatched_client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_client_fini(&unmatched_client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + bool is_available = false; + for (auto i = 0; i < timeout; ++i) { + ret = rcl_service_server_is_available(this->node_ptr, &unmatched_client, &is_available); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + if (is_available) { + // this should not happen + break; + } + std::this_thread::sleep_for(1s); + } + ASSERT_FALSE(is_available); + + rcl_client_t matched_client = rcl_get_zero_initialized_client(); + rcl_client_options_t matched_client_options = rcl_client_get_default_options(); + ret = rcl_client_init( + &matched_client, this->node_ptr, ts, matched_client_name, &matched_client_options); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + rcl_ret_t ret = rcl_client_fini(&matched_client, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + }); + + is_available = false; + for (auto i = 0; i < timeout; ++i) { + ret = rcl_service_server_is_available(this->node_ptr, &matched_client, &is_available); + ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + if (is_available) { + break; + } + std::this_thread::sleep_for(1s); + } + ASSERT_TRUE(is_available); +}